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ABSTRACT 

There are several common approaches used to obtain the 
kinematic and dynamic equations which describe the motion of 
robot manipulators. However, a problem arises when these 
conventional body oriented robot arm kinematic equations are 
used to simulate the manipulator motion. In this case, the 
jocobian matrix which relates the end effector motion to 
joint angle variations becomes singular when two successive 
arm links align. When the robot arm passes through these 
Singular points, the jacobian matrix is not invertible, and 
a result of this, the motion cannot be simulated. This 
thesis investigates how this situation can be avoided by 
using a Newton Euler approach to variable difinition, and 


using the equations interpretted in a fixed reference frame. 
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I. INTRODUCTION 


The study of robotics is a fairly new discipline. 
Although the roots of these studies and developments can be 
traced back to the 1940’s, the first commercial computer 
controlled robot was not introduced until the late 1950’s 
(Ref. 1]. Furthermore, as the theory developed, several 
common problemmatical methods have been widely accepted and 
used. 

When robot motion is studied, it is usually divided 
into two parts: robot arm dynamics and robot arm kinematics. 
While the kinematics problem deals with the geometry of the 
arm links, the dynamics problem deals with the study of 
forced motion. The dynamics problem is further divided into 
two parts: the direct dynamics problem and the inverse 
dynamics problem. In the inverse dynamics problem, link 
variables such as acceleration and velocity are known and 
the forces and necessary joint torques for the desired 
motion are calculated. In the direct dynamics problem, the 
joint torques are known and the accelerations and velocities 
of each joint are calculated. 

The kinematics problem is also divided into two parts: 
the direct kinematics problem and the inverse kinematics 
problem. The direct kinematics problem is, given a set of 


critical geometric joint and link variables for each of the 
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joint-link pairs and the joint angle vector, determine the 
position and orientation of the end effector of the 
manipulator. The Denavit-Hartenberg representation, which 
uses a homogeneous transformation matrix to describe the 
Spatial relationships between two adjacent rigid mechanical 
links is the most common method used to study the direct 
kinematics problem [Ref. 2]. The inverse kinematics problem 
is, given a desired position and orientation of the end 
effector of the manipulator and a set of critical geometric 
joint and link parameters, determine the corresponding joint 
angle vector; i.e., find all of the joint angles of the 
robot arm so that the end effector can be positioned in the 
desired location. 

A difficulty in the solution to the inverse kinematics 
problem arises when two successive links align [Ref. 3]. At 
these times the angle between two successive links becomes 0 
or 180 degrees, and the Jacobian matrix which relates the 
end effector motion to the joint variable variations cannot 
be inverted. This means that Tene cannot be simulated. 
Different approaches to this problem have been investigated. 
One method deals with the Newton-Euler approach with a 
moving coordinate system [Refs. 4, 5], another uses the 
Langrangian approach [Refs. 6, 7]. One method deals with a 
virtual work approach [Ref. 8]. Kane’s dynamics equations 


have been used due to computational efficiency [{Ref. 9]. 


26 


However, none of these methods have been able to overcome 
this singularity problem [Ref. 3]. 

Several methods have been proposed to avoid the 
Singular configuration. One method proposed to minimize the 
time near the singular points [Ref. 10], thereby reducing 
their effects. In another method, it was proposed to avoid 
these singular points by confining the motion [Ref. 11]. 
Other solutions deal with presenting equations that can 
translate the manipulator in the neighborhood of a 
Singularity through identification of Singular points 
beforehand [Refs. 12, 13, 14]. It has also been shown that 
the redundancy of robot manipulators is effective in dealing 
with the singularities [Refs. 15, 16, 17]. 

In this thesis the equations of motion are derived 
using the principles of Newtonian dynamics in terms of a 
globally fixed coordinate system to overcome the singularity 
problem. Each link is treated as a free body with forces 
and moments applied at the joints and free body analysis is 
used to derive the equations of motion. Although the 
equations are relatively long and the solution to the 
problem is computationally time consuming, it is shown that 
these equations do overcome the singularity problem. The 
direct dynamics andthe inverse dynamics problem are both 


Simulated. 


Vad 


II. THEORETICAL DEVELOPMENT 


A. THEORY OF THE SOLUTION 

To derive the non-singular equations of motion the 
Newton-Euler approach is used (Figure 1). Fach link is 
treated as a free body with forces and moments applied to 
it, weight has been disregarded. The globally fixed X Y Z 
coordinate system is used for the equations. All links are 
assumed to be rigid, so the effects of flexibility are not 
considered. All of the distances and the directions of the 
forces and moments have been based on the fixed coordinate 
System rather than a local coordinate system which moves 
with the link [Refs. 4, 5]. The link masses, the initial 
link positions and the orientations are assumed to be known 
parameters. As a result of equation derivation in the fixed 
reference frame, the moment of inertia is allowed to change 
with respect to time and is calculated for each small 
integration interval. This is opposed to keeping inertia 
constant as used in the local coordinate formulations. But 
it is assumed that the moment of inertia is constant in each 
small integration interval. This last assumption 
effectively linearizes the equations of motion so that a 
non-singular matrix inversion can be used to. solve the 


equation set. 
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Free Body Diagram of a Three Link Manipulator 


Reeure 1. 


ed 


To calculate the moment of inertia in each integration 
interval, the link direction cosine angles with respect to 
the fixed coordinate system were used. Acceleration of 
joint zero was input as zero. For each link the three 
linear acceleration components, three angular acceleration 
components and forces at each joint were considered to be 
the unknown variables. Based on the Newtonian dynamics and 
the manipulator kinematics [{Ref. 18], the equations were 


derived as follows: 
B. DYNAMIC EQUATIONS OF MOTION OF LINK ONE 
1. Sum of Forces Equations 
In the free body analysis of link one (Figure 1) the 
sum ae ne forces in the x direction is: 
=Fx = Fxi - Fx0O = Mlaxi (1) 
Similarly sum of the forces in the y direction is: 
2Fy = Fyl - FyO = Miayl CZa 


and the sum of the forces in the z2 direction is: 


22 = Hee EO yn Ssee leap: (3) 
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2. Joint Equations 
We begin by evaluating the joint equations at Joi me 
zero (Ref. 19, equation (8/4), pp. 423}. If the joint is 
sequested and analysis conducted at a point on link zero 
(Subscript a) and another at a point on link one (Subscript 
b) that is common to both, so when linked together they are 
equal. This results in two equations and the two unknowns 


wdi and wi. As a result: 

ha = 3Ko 
which is the acceleration at joint zero, and 

Ab = Al + (wdl x rb/Gl) + wl x (wi x rb/Gl) 
which is the acceleration of point bon joint one. Here 
rb/Gl is the distance from point b to the center of gravity 
of link one, and Al is the acceleration at the center of 


mass of link one or, 


rb/G@1i = (jx0-LCOGx1)i + (jy0-LCOGy1)j + (j2z0-LC0G21)k 


rb/Glx + rb/Gly + rb/Glz2 


After equating Aa and Ab and having the known variables on 
the right side of the equation and unknown variables on the 


left side the following sets of equations result: 
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Axi + wdyl(rb/Glz) - wdz1(rb/Gly) 
where MICO equals 


= wylwxl(rb/Gly) - (wyl)2(rb/Gl1x) 
+ w2lwxl(rb/Gliz) 


also 
Ayl + wdzl(rb/Glx) - wdxl(rb/Gliz) 
where MJCO equals 


= wa2lwyl(rb/Glz) - (wz1)2(rb/Gly) 
+ wxlwyl(rb/G1x) 


and 


Azl + wdxl(rb/Gly) - wdyl(rb/G1x) 
MKCO equals 


wxlwz1l(rb/G1lx) - (w21)2(rb/G1z) 


+ wylwzl(rb/Gly) 


3. Sum of Moment Equations 


Computing the sum of the moment 


center of gravity results in: 
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Aox - MICO (4) 


(w21)2(rb/G1x) 


Aoy - MJCO (5s) 


(wxl)2(rb/Gly) 


Aoz - MKCO (6) 


(wyl)2(rb/G1z) 


equations about the 


rhe = eer cee FOES (ri/Gi x Fi) - Ti + TO 


where the vector r0/Gil is the distance from joint zero to 


the center of gravity of link one and vector ri1/Gl1 is the 


distance from joint one to the center of gravity of link 


one, 


and 


50 


and 


in the x, y and z directions. Such that 


rovGil = rj0 =" rel 


ri/Gl ry - rel 


miler oiee s(x j0 — xal)i + (yj0 —- yGl)j + (2j0 - 2G1)k 


miei w(x jie xGd ji + (yjl = yGlj3j * (231 —- 2GT)k 


In the x, y and z directions the sum of moment equations 


are. 


2M1i in x direction = 
(yj0/G1)F20 + (2j0/G1)Fy0 + (yjl/Gi)F21l - (231/Gi)Fyi 
=~ Sine + TOX (7a) 


2M1 in y direction= 


Gauci txOlt (x590/7G1)b20 4+ (2gi7Gi)Fxi - (xjl1/Gi)F21 
ald yan es Oy, (8a) 
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SM1l in 2 directaon— 
(xj0/G1)Fy0 + (yjO/Gl1)Fx0 + (xj1/Gl)Byl - (yja7G1 )Fx1 
~Tiz + TOz (9a) 


From [Ref.19, equation (57), pp. 227] the sum of the moments 
about a fixed point that does not move with the body is 
equal to the time rate of change of angular momentum of the 
system (H) about the fixed point, 2M = H. In the present 
study we have let each link be a composite body of two 
elements. The angular momentum (H) for a composite body 
where the number of elements of the body is two, about the 

center of gravity of each link is Hi = 2 fRi x (wx 
Ri)]JMi, where Ri is the distance from the center of gravity 
of each link to the appropriate element (1 or 2) in the x, y 


and z2 direction. So: 


Hx = & [Ryi(wx(Ryi) - wy(Rxi)) - Rzi(wz(Rxi)- 
wx(Rzi)) ]Mi 

Hx = [R2yl(wx) -  Ryl(Rx1)(wy) -  Rzi(Rxl)(wz) + 
R2z1(wx)]M1 + [R@y2(wx) -  Ry2(Rx2)(wy)- 
Rz2(Rx2)(w2) + (R222)wx]M2 


If Ixx = Ry2 + Rz2 dn, 


and Ixy = ekxRyv adm 


and 4x2 = okeks amp 
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then: 
Hx = (Piers «~Lixytwy) - Lixz(wz)]M1 
tmlbbewxt ws) —- le2eyviwy) - [2xz(wz) jM2 
and 
HDx =" {1Pxx(wdx) - Ilxy(wdy) - I1lxz(wdz)]M1 
ay ie xx (Hae itzoeyveway) - Eexza(waz) |M2 (7D) 


by assuming the moment of inertia changes with time but is 
constant for a given time interval. 
By similar analysis it can be shown: 
Hy = [Rzi(wy(R2i) - w2a(Ryi)) - Rxi(wx(Ryi)- 


wy(Rxi) - wy(Rxi))JMi 


atderimiyy — hxe + RZe am, 
and tye =. RyRz dm, 

and Ixy = RxRy dm, 
then: 


Bee = fllyy(wdy) - ftlyz(wdz) - Ilyz(wdx) M1 
+ {I2yy(wdy) - IT2y2(wdz) - l2yx(wdx) JM2 (8b) 
and 
H2a=e2 | Weheereweerer) - wx(R21)) - Ryi€wy(Rzi)- 
wz(Ryi))JMi 
at Izz = Rx2 + Ry2 dm, 
So Bize=sibizz(w2)))= Llyz(wy) - Llzxtwx) |M1 


Set 2ZZZ20wz) = l2y Zz wae 2 2x (woe) | M2 
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then 
HDz = [Illz2z(wdz) - Ilyz(wdy) - Ilzx(wdx)]M1 
+ [I2z2(wdz) - I2yz2(wdy) - I2z2x(wdx) ]M2 (9b) 


Combining equations (7a) and (7b) and keeping known 
variables on the right side and unknown variables on the 


left side yields: 


SM1x = (-yj0/G1)Fz20 + (2j0/G1)Fy0 + (yj1/G1)F21 
- (231/G1)Fy1 - HDx = Tix - TOx (7) 


Combining equations (8a) and (8b) yields: 


2Mly = (-2j0/G1)Fx0 + (xj0/G1)Fz20 + (231/G1)Fxl1 
= (xjJ1I/GD)Fz2l = BDy el tye (8) 


Combining equations (9a) and (9b) yields: 


2Mlz = -(xjO/7G1)FyO + (yjO7GR EXO" + (xg1/Gl ra 
= (yjl1/Gl1)Fx1l = HD2 > =8112 az (9) 


C. LINK TWO EQUATIONS 


1. Sum of Forces Equations 
From the free body diagram (Figure 1) it follows 


that 
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Sex = Peo = cake (10) 


=Fy = Fy2 - Fyl = M2ay2 iby) 


Shoe ee = Fo) =Nizacc (32) 


2. Joint EFauations 


Analysis is conducted at joint one where similar 
equations are used aS in joint zero with a point on link one 


(a) and one on link two (b). For point a the equation is 


Aa = Al + wdil x ra/Gl + wi x (wl x ra/Gl) 


ra/Gl is a vector whose distance is measured from point a to 
the center of gravity of link one in the x, y and z 


direction. 


moy Gale =) (axl = LOCOGxi)i + (3yil - LOOGy1)j3 
+ jc ieee neOGzt ) ik 


ra/G1lx + ra/Gly + ra/Glz 


For point b the equation is: 


Mie wNe —Pewde % ED/GZ + we x (w2 x rb/G2) 


ort 


where rb/G2 is a vector whose distance is measured from 


point b to the center of gravity of link two. 


rb/G2 = (jx1l - LCOGx2)i + (jyl1 - LCOGy2) 3 
+ (jZ21 -etcoGzZ ik 
rb/G2x + rb/G2y + rb/G2z2 


Equating Aa and Ab and setting knowns and unknowns 


on the respective sides of the equation results in: 


Ax2 - Axl + wdy2(rb/G2z) - wdz2(rb/G2y) - wdyl(ra/Giz) 
+ wdazZl(ra/Gly)) =e ee Ce (135) 


MIC1 = wylwxi(ra/Gly) - (wyl)2(ra/Glx) - (w2l1)2(ra/G1ix) 
+ w2lwxl(ra/Gi1z) 


MIC2 = wy2wx2(rb/G2y) - (wy2)2(rb/G2x) - (w22)2(rb/G2x) 
+ w22wx2 (rb/G2z2) 


Ay2 - Ayl + wdz2(rb/G2x - wdx2(rb/G2z2z) - wdzi(ra/G1x) 
+ wdxl(ra/Giz) = MJC1 - MJC2 (14) 


MJC1 = wa2lwyl(ra/Giz) - (w21)2(ra/Gly) - (wxl1)2(ra/Gly) 


+ wxlwyl(ra/G1ix) 
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Or 3: 


where 


Pee. = Wwaewy2(6b/G2z) = (w2e)2(rbd/G2y) ~ (wxd)2(rb/G2y) 


+ wx2wy2(rb/G2x) 


AZ2 - AZ1 + wdx2(rb/G2y) - wdy2(rb/G2x) - wdxl1l(ra/Gly) 
+ wdyl(ra/Glx) = MKC1 - MKC2 (15) 


MKC1 = wxiwzl1(ra/G1ix) - (wx1)2(ra/G1z) - (wyl1)2(ra/Giz) 


+ wylwzli(ra/Gly) 


MKC2 = wx2w22(rb/G2x) - (wx2)2(rb/G2z) - (wy2)2(rb/G2z) 


+ wyewz2(rb/G2y) 
3. Sum of the Moment Equations 
These equations have a similar development as that 
nk one: 
Signer aie ace) x Pil + (rj2/G2Z) x F2 + T1 - T2 


wal /Gae (Silence eet (yj VG2y 9+ t2gi - 2G2)k 


mie Glee (xj2.- <Gce)i + (yj2 — yG2)3 * (232 - 2G2)k 


Pirie — (Y j1 ViGoyEalet sole = ead) ry 1 
+ (yj2 - yG2)Fz22 - (2j2 - 2G2)Fy2 


lle © 2X (16a) 
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2M2y = - (239 2G2)Pxl + (xgieeae Ze 

+ (232 = 2ZGZ)E x2 eax icra ee 

+ Tly = f2y (17a) 
2M22 = = (xjl - xG2)eea 45 (y 31 GZ xd 

+ (xj2 = xGZ)By2e—] (yjZe-wye xe 

ois = leg (18a) 


From angular momentum equation developed for link one, it 


can be shown for link two: 


2M2x = HDx | (16b) 
=M2y = HDy (17b) 
VAR = alle (18b) 


Combining equations (16a) and (16b) the following 
result: | 
- (yjl - yG2)F2l + (29! = 262) yi 2 Fee 
- (2392 -—- 2G2)FyZ — ADx eae 1 xe (16) 


Combining equations (17a) and (17b) yield the 
following result: 
- (231 - 2G2)Fx1l + (xjl1 - xG2)F2il + (232 - 2G2)Fx2Z 


- (xj2 —- xGZ)Fa2— Dy = tee (17) 
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Combining equations (18a) and (18b) yield the 
following result: 
=- (xjl - xG2)Fyl + (yjl - yG2)Fx1l + (xj2 - xG2)Fy2 


pelvic semi Uxee eit = —Tiz + T2z2 8) 


Dy LINK THREE EQUATIONS 


1. Sum of Forces Equations 


Following Similar logic from that previously 


developed: 
Dx] = PXoeo Meax3 (19) 
Pov eve = Maave (20) 
pases - H22)- WS=9M3az3 C2) 


2. Joint Equations 


With point a on link two and point bon link three 
one gets for joint equations at joint two: 


Aa = A2 + (wd2 x ra/G2) + w2 x (w2 x ra/G2) 


where ra/G2Z is a vector whose distance is measured from 
point a to center of gravity of link two in the x, y and 2 


direction. 
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ra/G2 = (jx2 = h@OGx2Z)i + (jvc aebeeGyz 
+ (2c pee Oade ik 


ra/G2x + ra/G2y + ra/G2z2 


For point b 


Ab = A3 + wd3 x rb/G3 + w3 x (w3 x rb/G3) 


where rb/G3 is a vector whose distance is measured from 
point b to center of gravity of link three in the x, y and z 
direction. 
rb/G3 = (jx2 = LCOGx3)}i + (j¥2 seca 
+ (j22 - LC0Gz23)k 


rb/G3x + rb/G3y + rb/G3z 


Equating Aa and Ab and setting knowns and unknowns on the 


respective sides of the equation results in: 


Ax3 - Ax2 + wdy3(rb/G3z) - wdz3(rb/G3y) - wdy2(ra/G2z) 
+ wdz2(ra/G2y) = MIC3 - MIC4 — (es 


MIC3 = wy2wx2(ra/G2y) - (wy2)2(ra/G2x) - (w22)2(ra/G2x) 


+ w22wxe(ra/G2z) 


MIC4 = wy3wx3(rb/G3y) - (wyv3)2(eb/7Gox) (w23)2(rb/G3x 


+ wz23wx3(rb/G3z) 
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where 


my Ol Aye )t ewadss( re/Gox)) — wdaxs(rb/G3z) - wdzd(ra/Gex) 
+ wdx2(ra/G2z) = MJC3 - MJC4 (23) 


MJC3 = wz2é2wy2(ra/G22z) - (w22)2(ra/G2y) - (wx2)2(ra/G2y) 


+ wxewy2(ra/G2x) 


MJC4 = w23wy3(rb/G3z) - w223(rb/G3y) - w2x3(rb/G3y) 
+ wx3wy3(rb/G3x) 


AZ3 - AZ2 + wdx3(rb/G3y) - wdy3(rb/G3x) - wdx2(ra/G2y) 
+ wdy2(ra/G2x) = MKC3 - MKC4 (24) 


MKC3 wxew2e(ra/G2x) - (wx2)2(ra/G2z) - (wy2)2(ra/G2z) 


+ wx2wy2d(ra/G2y) 
MKC4 = wx3w23(rb/G3x) - (wx3)2(rb/G3z) - (wy3)2(rb/G3z) 
+ wy3w23(rb/G3y) 


3. Sum of Moment Equations 


As in the development of the equations for link one: 


Pue= (rr jicgjas) x He + T2 


meyer (xac — xG3)a + (yj2 = yG3)3 + (232 - 2G3)k 
mee / G3 Heyye/Gs te2)2/7G3 
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2M3x 


2M3y 


2M3zZ 


= (-yj2/G3) 8223 (2352/Gon 2 (25a) 


= (-232/G3 )EXZ (2, Gs ey, (26a) 


(-xj2/G3)Py2 + (yj32/Gapee2 eee (27a) 


From the angular momentum theory: 


2M3x 


2M3y 


2M3z 


Combining 


= HDx 


= ey, 


Serie 


equations 


(=V.327 Ge) tiaZaee 


Combining 


equations 


(-232/G3)Px2 + 


Combining equations 


( 23)9))) 
(26b) 
(27b) 
(25a) and (25b) the following results: 
(z232/G3)By2 — AD 12x (259) 
(26a) and (26b) the following results: 
(3012 /G3) F225 - 5 ya ee (26) 


(27a) and (27b) the following results: 


(-xj2/G3)fy2 + (yj27 Geol x2 Sez — ee iz (2%) 
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III. COMPUTATIONAL APPROACH 


A. PROGRAM MATRICIES 

The Dynamic Simulation Language (DSL) was’ used to 
Simulate the motion. This computer code was compiled on an 
IBM 3033 computer by using the FORTVS compiler and all of 
the calculations have been done in double precision. The 
entire simulation process is shown in Figure 2 and is 
discussed below. 

The principle program matrix, Matrix A (MATA, 27*27), 
was created from the coefficients of the unknown variables 
in equations 1 to 27. In the simulation of the direct 
dynamics problem, a corresponding 27*1 Matrix B (MatB) was 
generated from all known variables, also from equations 1 to 
ZA A subroutine CPROD was used to perform all the cross 
product terms required in the main program. The resulting 
equations are shown in Figure 3, in the final matrix form. 
During a simulation time step, the link inertias, the link 
velocities and the link positions were all assumed constant. 
IMSL subroutine LEQT2F was called to invert the matrix A and 
get the generalized solution x from Ax = B. This subroutine 
uses Gaussian elimination with iterative improvement to get 
a high accuracy solution to the problem. The output from 
LEQT2F then returns as MATB, which contains the solution to 


the equations. The outputs were used by DSL to integrate 
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the linear and angular accelerations of each link to get the 
linear and angular velocities respectively. The linear 
velocities for each link were next integrated to get the 
linear displacements of center of gravity of each link. 
Although the linear velocities in the fixed reference frame 
can be integrated to get the linear displacements, this idea 
is not true for the angular displacements@iers ) 20yeat 

To get the angular displacements, a set of 
transformation matrices must be used on the velocities, then 
the motion can be integrated. That is, the angular 
velocities of each link in the fixed reference system must 
first be converted into equilavences in a body fixed 
coordinate system then into body Euler rates and Euler 
angles to define the motion unambigously. In this thesis, 
the body coordinate velocities are called Bratel, Brate2 and 
Brate3 for the link one, link two and link three 
respectively. To convert these velocities into the Euler 
rates, another transformation matrix is used. That is, the 
transformation matrix is multiplied by body rates to get the 
BFuler angle rates for each link. These later rates are 
defined as the Yaw rate (about the x axis), the Pitch rate 
(about the y axis) and the Roll rate (about z axis). These 
rates are called as Ratel, Rate2 and Rate3 for link one, 
link two and link three. After the transformation of 
velocities to the Euler rates, they can be directly 


integrated to get the Euler angles. In this thesis, these 
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angles are called the Yaw, pitch and Roll angles about the x 
y 2 axis respectively [Refs. 2, 20]. 

This convention is very important and should not be 
mixed with another set of Euler angles described differently 
maecune literature [Refs. 3, 20]. In addition to that, the 
order of the rotation must be decided beforehand. This is 
true because the orientation of objects is different when 
they are rotated in a different order, i.e., first the 
rotation about x axis, then a rotation about the y axis, 
finally a rotation about the 2 axis will produce a different 
orientation in space than the one which was defined and used 
in this thesis (z, y, then x). The transformation matrices 
used here are valid as long as the assumed order of the 
rotation is retained. 

In the literature, a quite different set of angles is 
used to describe the orientation [{Refs. 2, 3]. | While some 
of these angles define the orientation with respect to a 
non-orthogonal coordinate system some others may define with 


respect to an orthogonal system. Euler angles define an 


independent set of coordinates system which are not 
orthogonal. Therefore, all three coordinates are 
independent from each other and velocities ay ooloale 


coordinate system can be directly integrated to get the 
relevant angles. They describe the unique orientation of 
the body in space. The orthogonal set of coordinate axes do 


not form an independent coordinate system. This is true 


4Y 


Since the three axis have a certain relation with each other 
in any position, i.e., direction cosine angles have a unique 
relation in a fixed reference system and cannot be obtained 
by integrating any velocity in an orthogonal coordinate 
system. The velocities in an orthogonal coordinate system 
must thus be converted to a nonorthogonal coordinate system 
(e.g. Euler angle rates) prior to integration. 

After Yaw, Pitch and Roll angles are calculated, it is 
possible to go back and express the orientation of the body 
with the direction cosines in an orthogonal coordinate 
system. The columns of the transformation matrix from one 
orthogonal set of axes to another describes the orientation 
of the new coordinate axis with respect to old coordinate 
system. So, a transformation matrix can be used to get the 
direction cosine angles. The direction cosines of each link 
are then used to calculate the moment of inertia of the 
links. The variation of a link inertia with respect to time 
was shown in Figure 4 as it was calculated during a 
Simulation run. The demas tien of the transformation 
matrices is shown in Appendix A. 
ie CONSTRAINTS IN THE SIMULATION PROGRAM 

In the development 8 the equations, thus far, each 
link has been treated such that it can move in space without 


any constraint. For most cases, however, degrees of freedom 
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of each link must be reduced so that the link can move only 
in the direction permitted by its joint. 

In the simulation of the direct dynamics problem, the 
base rotation is transmitted to the second and third links 
for the three revolute joint arm which was studied. This 
was simulated by allowing the first link to rotate only 
about Z axis. At the same time, the rotational rates of the 
second and third links about the Z axis were made equal. 

To make any of the simulation variables zero, meaning 
no variation in that direction, one zeroes the related rows 
and columns in MatA putting 1 on the diagonal. At the same 
time, if the same row in MatB is made zero, the 
corresponding mathematical expression for this equation will 
be in the form of 1 * KX = 0, and a result of this, X will be 
equal zero. This idea can also be applied to MatA and MatB 
to make two variables equal so that X1 - X2 = Q. Thus, the 


above motion was Simulated by constraint. 


Ce PROGRAM VALIDATION 

The validation of the inverse dynamics problem has been 
conducted in several cases. In this approach the idea was 
to choose an angular acceleration such that at a certain 
time, two of the three links would align. In other words, 
the links would be in a Singular position at this time, and 
if the simulation procedure worked, the singularity problem 


would have been avoided. 
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The validation procedure is shown in Figure 5. Fos 
this procedure link two angle was chosen as 989 = (Pi/2) * 
Sin(pi/2) * Time. This time dependent function has a period 
of 4 sec and an aapinteade of 90 degrees. The second 
derivative of this function is 8 = - ((pix**3)/8) * sin(pi/2) 
* Time and corresponds to the angular acceleration of the 
link. This value was input as the theoretical angular 
acceleration in the simulation program, and corresponding 
linear accelerations and forces at each joint were 
calculated. The other two links were forced to have zero 
rotational velocity throughout the simulation. 

To apply a corresponding torque at the joint, MaTA and 
MatB were multiplied and a right hand side matrix DQ (27*1) 
was obtained (MATA * MATB = DQ). This matrix DQ (27*1) was 
used to solve the simulation equations in the form of 
AX = DQ. The vector X (that is, theta) was fedback in the 
loop and the theoretical and the calculated values of theta 
were compared. 

The above discussion has been implemented in three 
different initials configuration as shown in Figure 6. To 
force the arm links to the various singular points, several 
different plane motions were simulated. For each 
pene urat on: three different angular motions were input 
for link 2, or as can be seen from Figure 6, for each 
configuration, one angular velocity caused a spinning motion 


of the link about the axis with which it was initially 
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Figure 5. Validation Procedure Flow Chart 
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aligned, while the other two produced a plane motion 
according to direction of the applied angular motion. The 
angles between two successive links were measured for each 
motion. Figure 7 shows the angle variation between link 1 
and link 2, and link 2 and link 3 corresponding to an 
angular acceleration applied in the X direction for 
configuration A. As can be seen from Figures 7 and 8, two 


successive links pass through the singular points every 2 


seconds, i.e., they align and the angle between links 
becomes either 0 or 180 degrees. (The singular points are 
marked on the graph). Figure 8 shows the angle variations 


for an angular motion applied in the 2Z direction for 
configuration A. In this case, it is obvious that the angle 
between link 1 and link 2 is always constant (90 degrees). 
The second graph on Figure 8 shows the angle between link 2 
and link 3 now, singularity occurs on the Z motion, with the 
singularities marked as in Figure 8. Figure 7 and Figure 8 
are representative of the data obtained in the validation 
procedure which analyzed nine possible motions oA link ez 
leading the singularity. This data showed that singularity 
in these directions could be overcome, and a solution to the 
problem exists using this approach. 

For each run, the error between the theoretical and the 
simulated value of Theta was computed. Figure 9 shows the 
percent error for the X motion for configuration A (Figure 7 


Data). The trend of the error is representative of every 
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4.0 


1.5 


1.0 


0.0 


TIME (SEC) 


case investigated. The figure shows that, due to nature of 
the numerical integration, the error slightly accumlates 
during the simulation, but still has very small value. This 
proves that the direct dynamics problem can be solved very 
accurately by Newton-Euler approach in a fixed coordinate 


system. 
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IV. RESULTS AND RECOMMENDATIONS 


A dynamic model of a three link, rigid revolute joint 
manipulator has been developed in this thesis, as a 
general computer program package. 

Several runs’) for different apeenieal configurations were 

Simulated and the singularity problem was investigated. 

Theoretical and calculated values of angular positions 

were compared. It was proved that the singularity 

problem could be overcome by using a Newton-Euler 
approach in a fixed coordinate system. 

The following recommendations are provided: 

a. Enhance the code and make it more interactive. 
That is, let the user specify the constraints he 
wants to apply on each link by answering 
interactive questions before the actual simulation 
run starts. Thus, the motion can be simulated 
With different constraints without going into the 
code and changing the relevant parameters. 

be Adapt the code for use in a microcomputer. Add a 
subroutine in the program to invert the matrix a 
Thus, the code will be more independent from 
outside routines and more adaptable to other 


computer systems. 
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Validation of the approach via actual experimental 
tests in crucial. This will establish a way of 
developing Beevers constants for subsequent 
controller design and provide a basis for 
compensation of gravity effects. Determining 
these constants for the code will make the 
simulation auseeen more concrete and will provide 
more physical insight. 

Finally, develop a controller for a manipulator 


which makes use of the present algorithm for 


validation and design. 
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APPENDIX A 


DERIVATION OF THE TRANSFORMATION MATRIX FROM 
FARTH FIXED COORDINATE SYSTEM TO BODY FIXED COORDINATE SYSTEM 


The angular velocity terms obtained by integration of 
the angular acceleration terms are with respect to an Earth 
fixed coordinate system. To define the Euler angles which 
are called Yaw Pitch and Roll angles in this thesis, we 
have to establish an appropriate body fixed coordinate 
system. Thus, U, V and W is a right hand coordinate system 
[Ref. 20] with its origin fixed at the center of gravity of 
a link. The U, V, W coordinate system is initially oriented 
such that the angles between two coordinate system axes are 
Simultaneously reduced to zero, i.e., i, -j3, kk, axis are 
parellel to the I, J, K respectively. 

If a rotation from X Y Z coordinate system to the U V W 
coordinate system is accomplished by first rotation about K 
axis (roll), then about J axis (pitch) and finally about I 
axis (yaw), it follows that for any arbitrary point in the X 
Y Z coordinate system, the corresponding coordinates in the 


UV W system are; 
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<< ON 


where MatR is a 3*3 matrix. 


To get the transformation matrix, we need to examine each 
rotation separately. 


Rotation about the Z axis; 


uz=Xcd+ Ys 
V=-Xso + {co 


Ws Z 
U ch so o]| X 
Vi=l-s§ cp O11 Y 


af! eal 





Rotation about the Y axis; 
ua XcO~ ZSeC 
Vv: Y 
W: XSO0+2C0 





Rotation about the X axis; 


U= X 

Ve {CY + Z2SY 

Wa-YSY4 ZCY 
uu { O O x 
J —| O cw SY Y 


Wl | oOo -sp cH) 2 





L 


By multiplying three rotation matrix together; 


cecd cEes¢d -SOe 
MATR «= SYsecg- cso SYsOsgecycF SHce 
Cyseco + SPs Cyses¢ -SUcP Cyce | 
where C = COS 
S = Sin 
T = Tan 


The transformation matrix from body fixed to Euler 


coordinate system is obtained as below [Ref. 20}. 


fe) cy -sy 
{ TeSsy Tecd 
Oa Sees ae fa 


The angles discussed above are shown in Figure 10. 
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z,, roll axis 


4 


y, pitch axis 
=) 


X, Jam axis 


(9) 





(>) 


Figure 10. Critical Angles 
(a) Euler Angles, Body Coordinates 


(b) Direction Cosines, Global Coordinates 
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APPENDIX B 


THREE DIMENSIONAL DIRECT DYNAMICS SIMULATION PROGRAM 


TERMINAL 

METHOD ADAMS 

PRINT .05,DRCANX(1-3) ,DRCANY(1-3) ,DRCANZ(1-3),... 
tO. O, JcOe In JYl,JZ1,0K2, 32,322 

















LCOGX (1-3) , LCOGY(1-3) ,LCoGz(1-35 
CONTROL FINTIM =2.0, DELMAX =.1, DELPRT = .05 
D DIMENSION MATA(27,27) ,MASS(3,2),L(3 2) BH (3,2) RY(3,2) .RZ(3 2) 
D DIMENSION IXX(3,2),1XZ(3,2) ,1X¥(3,2), 1¥V(3,2) ,1Y2(3,2) ,122(3,2) 
D DIMENSION Beta yee) MATE 3,2) MATSR(3. 3) 
D DIMENSION MAT1T(3,3),MAT2T(3.3).MAT3T(3,3 
D INTEGER IER,1I,J,M,K,P,N,1A, IDGT,A 
EXCLUDE IA, IDGT,IER,I,J,M,K,P.N.A 
ARRAY MATB(27),LCOGX(3) , LCOGY(35 ,LCOGZ(3) 
ARRAY VECTAO(3),VECTBO(3),VECTA1 (3), VECTB1(3) , VECTA2(3),VECTB2(3) 
ARRAY Bras) Weta) aeme (a) Wi (a) .W2t3) a(S) 
ARRAY RATE1(3),RATE2(3) ,RATE3(3) ,BRATE1(3), BRATE2(3), BRATE3(3) 
ARRAY RBG1(3) ,RAG1(3),RBG2(3),RAG2(3),RBG3(3) 
ARRAY SUMHDX(3),SUMHDY(3),SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) , WKAREA(850) 
PReey 1RkT (3), 1YYT(3) 1Z2T(3), DXYT(3)), 1X27 (3) ,1YZT(3) 
ARRAY YAWANX(3) ,PTCANY(3),ROLANZ(3 
ARRAY DRCANX(3),DRCANY(3),DRCANZ(3 
ARRAY DRCRAX(3),DRCRAY(3) ,DRCRAZ(3 
ARRAY DRCSX(3) ,DRCSY(3) ,DRCSZ(3) 
D DATA MATA/729 * 0.0D0/ 
INITIAL 
* INPUT PARAMETER CONSTANTS 
A = 5.0D0 
P = 0.0D0 
W = 2.0D0 * PI 
IDGT = 3 
G=0.0D0 
N=27 
M=1 
IA =27 
* INPUT JOINT LOCATIONS IN METERS 
JXO = 0.0D0 
JYO = 0.0D0 
JZ0 = 0.0D0 
JX1 = 0.0D0 
JY1 = 0.0D0 
i2i= tobe 
JX2 = 0.0D0 
JY2 = 1.0B0 
JZ2 = 1.0D0 
* INPUT TORQUE CONSTANTS 
TOX = 0.0D0 
TOY = 0.0D0 
TOZ = 0.0D0 
T1Y = 0.0D0 
T1Z = 0.0D0 
T2Y = 0.0D0 
T2Z = 0.0D0 
x INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 
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* FOR EAC NK ENDS 


0.50D0 
Q.50D0 
Q.50D0 
0.50D0 
Q0.50D0 
0.50D0 


* INPUT MASS AT LINK ENDS IN KILOGRAMS 


NO NO NON NO DO 
un 
0 
Oo 


* TNE UTS CEG D OMEGA DOT, YAW, PITCH, AND ROLL ANGLES 


par 


O00 O9O0000 
‘e) 
oO 
o) 


WDZ 


YAWANX(I 
PTCANY (I 
ROLANZ (I 


30 CONTINUE 


YWRX1 
Fe Osers il 
RLRZ1 
YWRXKZ 
PUR YZ 
RLRZZ 
YWRK3 
PIRYS 
RLRZ3 


* INPUT LOCATION OF LINK CENTERS OF GRAVITY 


Te et 
rd 
4 
oO 
as) 
a 
KG 
JW) GI NONI NF FF 
oe ee ee ee a 
S, 
ea 
Q 
BY) 
ip 


€ 
" £C0GZ(3) = 1.0D0 
Zg0 =" LecezS) 


* INPUT MASS OF EACH LINK IN KG AND COMPUTE WEIGHTS IN NEWTONS 


= 5.0D0 
MASSZ = 5.0D0 
MASS3 = 5.0D0 


WGl = MASS1*G 
WG2 = MASS2%G 
WG3 = MASS3%G 


im INPUT ACCELERATION OF JOINT ZERO 
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AQX 
AQY 
—A0Z 0.0D0 


Ts 
© 
© 
7] 
oO 


rar 
MATA(I,J) = 0.0D0 
50 CONTINUE 
40 CONTINUE 


DO 60 = 1,27 
MATS (I) = 0.0D0 
60 CONTINUE 
x INITIALIZE THE TRANSFORMATION MATRICIES AND VELOCITIES 


DOD6sa l= 1775 
DO 64 J = 1,3 





RATE1(I = Q.0D0 
RATE2(I = Q.0D0 
RATE3 (I = Q.0DO 
BRATE1(I = 0Q.0D0 
BRATE2 : = Q.0D0 
BRATE3 (I = 0.0D0 
MAT1T (I,J) = 0Q.0ODO 
PAtZ? (1 Jp m” 0.0D0 
MAT3T (I,J) = 0.0DO 
MATIR (1,3) = 0.0DO 
MaAGZR (1,3) = O.0BG 
MATSR (I,J) = O50D0 

64 CONTINUE 

63 CONTINUE 


DERIVATIVE 

NOSORT 
CALL ERRSET (208,256,-1,1,1) 
EVELQO = 0 


L 
CALL JERSET(LEVELO, LEVLDO) 
x INITIALIZE MATRIX A AND B TO ZERO 


DOOR = 1, at 
Smoege = 1 527 
MATACI, 3 = 0.0D0 
80 CONTINUE 
70 CONTINUE 


DOy3o i 


TB (I) 


Load 
MA = 
30 CONTINUE 


0.ODO 


INPUT JOINT EQUATIONS 
= JOINT ZERO EQUATIONS 
“3 AB = AG1 + (WD1 X RB/G1) + Wl X (W1 XK RB/G1) 


VECEAO(2 = WDX(1 
3 


VECTAO WDY(1 
VECTAO WDZ(1 


Ret {2} JZOn = Leosr 1} 











RBG1(2) = JY¥O - LCOGY(1 
RBG1(3) = JZ0 - LcoGztl 

CALL CPROD(VECTAO,RBG1,MIAO,MJAO ,MKAO) 
VECTAO(1) = W1(1 
VECTAO(2) = W1(2 
VECTAO(3) = W1(3 


CALL CPROD(VECTAO,RBG1,MIBO,MJBO ,MKBO) 
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+ 


+ 


VECTBO(2 


VECTBO | if } 
VECTBO(3 


CALL CPROD(VECTAO,VECTBO,MICO,MJCO,MKCO) 
JOINT ONE EQUATIONS--- 
AA = AG1 + (WD1 X RA/G1L) + Wl X (W1 X RA/G1) 














VECTAL(1) = WDX(1 
VECTAL(2) = WDY(1 
VECTAL(3) = WDZ(1 
RAG1(1) = JX1 - LCOGX(1 
RAG1(2) = JY1 - LcoGy(1 
RAG1(3) = JZ1 - LCOGZ(1 
CALL CPROD(VECTA1,RAG1,MIA1,MJA1,MKA1) 
VECTA1(1) = W1(1 
VECTA1(2) = W1(2 
VECTA1(3) = W1(3 
CALL CPROD (VECTA1,RAG1,MIB1,MJB1,MKB1) 
VECTB1(1) = MIB1 
VECTB1(2) = MJB1 
VECTB1(3) = MKB1 


CALL CPROD (VECTAI ,VECTB1 ,MIC1,MJGi Mew) 
AB = AG2 + (WD2 X RB/G2) + W2 XK (W2 X RB/G2) 





VECTA1(1) = WDX(2 
VECTA1(2) = WDY(2 
VECTAL(3) = WD2(2 
RBG2(1) = JX1 - LCOGX(2 
RBG2(2)°= JY1 - LCOGY(2 
RBG2(3) = JZ1 - LcoGZ(2 
CALL CPROD (VECTA1,RBG2,MIA2 ,MJA2 ,MKA2) 
VECTA1(1) = W2(1 
VECTA1(2) = W2(2 
VECTAL(3) = W2(3 
CALL CPROD (VECTA1,RBG2,MIB2,MJB2 ,MKB2) 
VECTB1(1) = MIB2 
VECTB1(2) = MJB2 
VECTB1(3) = MKB2 


CALL CPROD (VECTA1,VECTB1 ,MIC2 ,MJC2,MKC2) 


JOINT TWO EQUATIONS 


AA = AG2 + (WD2 X RA/G2) + W2 X (W2 X RA/G2) 











VECTA2(1) = WDX(2 
VECTA2(2) = WDY(2 
VECTA2(3) = WDZ(2 
RAG2(1) = JX2 - LCOGX(2 
RAG2(2) = J¥2 - LCOGY(2 
RAG2(3) = JZ2 - LCOGZ(2 
CALL CPROD (VECTA2,RAG2,MIA3,MJA3,MKA3) 
VECTA2(1) = W2(1 
VECTA2(2) = W2(2 
VECTA2(3) = W2(3 | 
CALL CPROD (VECTA2,RAG2,MIB3,MJB3 ,MKB3) 
VECTB2(1) = MIB3 
VECTB2(2) = MJB3 
VECTB2(3) = MKB3 


CALL CPROD(VECTA2,VECTB2,MIC3,MJC3 ,MKC3) 
AB = AG3 + (WD3 X RB/G3) + W3 X (W3 X RB/G3) 
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x 


VECTA2(1) = WDX(3 

VECTA2(2) = WDY(3 

VECTA2(3) = WDZ2(3 

RBG3(1) = JX2 - LCOGX(3 

RBG3(2) = J¥2 - LcoGY(3 

RBG3(3) = JZ2 - LCOGZ(3 

CALL CPROD (VECTA2,RBG3,MIA4, MKA4, MKA4) 

VECTA2(1) = W3(1 

VECTA2(2) = W3(2 

VECTA2(3) = W3(3 








CALL CPROD (VECTA2,RBG3,MIB4,MJB4,MKB4) 


WECTB2 
VECTSZ 


2 
3 


vectsa (2) = MIB4 


MJB4 
= MKB4 


CALL CPROD (VECTA2,VECTB2,MIC4,MJC4,MKC4) 


SUM OF MOMENTS EQUATIONS 


DO 100 I =1,3 
GCirUGEena,n DOT X,Y; R 


ex) = =n 
ee) = ae 
RY(I,1) = -L 
RViET 2) = £ 
RZ(I,1) = -L 
Rai 2) = 2 
TX (T. 1) = Best. 1) x 
IXX(I,2) = MASS(I,2) * 
ie =i xx(l. 1) 4+ 
Trees tT) «LE. .020) 
irom) = .020 
ELSE 
iounc i) = 1XXT (I) 
END IF 
aCe = fet) x 
IXY(I,2) = MASS(I,2) * 
TXVn = TxXY(i,1) + 
aed = esti} * 
TXZ(Gee y= MASS(I,2) * 
Ze TXZ (1,1) + 
sey - state x 
HDX(2) = WDX(2) * IXX 
ase - MASS (T, 1) x 
IYY(I,2) = MASS(I,2) * 
IYYT(I) = Iyvy(I,1) + 
IF (IYYT(I) .LE. .020) 
inane = ,020 
ELSE 
Ty d)))) = IYYT(I) 
END IF 
i add = sree k 
IyZ(I,2) = MASS(I,2) * 
iyzea = 1Y2(1.1) + 
se = Hage * 
HDY(2) = WDY(I) * Ivy 
eA = ae * 
1Zz(I,2) = MASS(I,2) * 
ame = 127 (1 1) + 
(ee iZZT (1 )eeLE, 5020) 


DOs, 2,8 Dole 


DCOS (DRCRAX 
DCOS (DRCRAX 
DCOS (DRCRAY 
DCOS (DRCRAY 
DCOS (DRCRAZ 
DCOS (DRCRAZ 


RY(I,2} co Ry(T,1)} + (Rz(T, 1) 7 RZ(1,1))) 
RY(I,2) * RY f72 + (RZ(I,2) * RZ eZ 
(1,2) 

EN 


> a, i, a 
HHHHAHH 


RX(I,1) * RY(Z.1) 

Reid. 2) SeReed, 2 

IX¥(I,2 

Pact) 1) Bx(T 41) 

PANT 2) * RXCE oe 

ea Gee 

z.1}- HD2(T) x THz (1.1) - HOY (T} x Ixy (1.2) 

7 2 NDZi Eee TRZ(T 2 = WOYiT) * IXY(I,2 

Bea x eae + oy x Pe mee A 
Bet Oye Re Ro ee RZ(1,2 

IYY(I,2) 

THEN 

SES AT Nan ces Sea 

Ryd 2) * R2i tee 

Tyz(I,2 

¢2) : Hoes. * oe} 2 wg) T) * Hatta 

io Wx 1 eee 2) = WZ * IYZ(1.2 
aR x sh + Eater ae By (2 222} 
Reto een to) + CRY(T 2 2 

TAAL) 

THEN 


100 





TZ2T(1) = ".070me 
ELSE 
TZZT(1) = Zama) 
END IF 
rey = AES * eet - with * eee - oT x eztte 
HDz(2) = WOZ(IL) * IZZ(I,2) - WDX(I) * Ixz(I,2) - woY(I) * Iyz(I,2 
SUMHDX(I) = HDX(1) + HDX(2 
SUMHDY(I) = HDY(1) + HDY(2 
SUMHDZ(I) = HDZ(1) + HDZ(2 
CONTINUE 
ENTER CONSTANTS INTO MATRIX A 
LINK ONE 
SUM OF FORCES IN THE X DIRECTION 
MATA dL) = 1.0D0- 
MATA(1,4) = MASS1 
MATA(1,10) = -1.0D0 
MATB (1) = 0.0D0 
SUM OF FORCES IN Y DIRECTION 
MATA Z+2) = 1.0D0 
MATA(2,5) = MASS1 
MATA(2,11) = -1.0D0 
MATB (2) = 0.0D0 
SUM OF FORCES IN Z DIRECTION 
MATA 3.3) = 1.0D0 
MATA(3,6) = MASS1 
MATA(3,12) = -1.0D0 


SUM OF FORCES LINK ONE EQUAL 


MATB (3) = 


-WGl 


EQUATIONS AT JOINT ZERO 
IN THE 4% DIRECTION 





MATA(4,4) = 1.0D0 
MATA(4,8) = a 
MATA(4,9) = -RBG1(2 
MATB(4) = AOX - MICO’ 
IN THE Y DIRECTION © 
MATA(S,5) = 1.0D0 
MATA(S5,7) = pe), 
MATA(S5,9) = RBG1(1 
MATB(5) = AOY - MJCO 
IN THE Z DIRECTION 
MATA(6,6) = 1.0D0 
MATA(6,7) = Be 
MATA(6,8) = -RBG1(1 
MATB(6) = AOZ - MKCO 
SUM OF MOMENTS EQUATIONS FOR LINK ONE IN THE X,Y,Z DIRECTIONS 
MATA(7,2) = RBG1(3 
MATA(7,3) = -RBG1(2 
MATA(7,7) = -IXKT(1 
MATA(7,8) = IXY¥T(1 
MATA(7,9) = IXZT(1 
MATA ,iae = -RAG1(3 
MATA(7,12) = RAG1(2 
MATB(7) = TLX - TOX 
MATA(8,1) = -RBG1(3) 
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MATA(8,3) = RBG1(1 
MATA(8,7) = IXYT(1 
MATA(8,8) = -IYYT(1 
MATA(8,9) = I¥ZT(1 
MATA(8,10) = RAG1(3 
MATA(8,12) = -RAGI(1 
MATB(8) = T1Y - TOY 
MATA(9,1) = RBG1(2 
MATA(9,2) = -RBG1(1 
MATA(9,7) = IXZT(1) + IXZT(2) + IxX2ZT(3 
Meroe = WivzZind)) + IYZT(2) 4 1yzZT(3 
MATA(9,9) = -IZZT(1) - IZZT(2) - 122T(3 
MATA ae = -RAG1(2 
MATA(9,11) = RAG1(1 
MATB(9) = T1Z - TO0Z 

LINK TWO 

SUM OF FORCES IN X DIRECTION 
MATA(10,10) = 1.0D0 
MATA(10,13) = MASS2 
MATA(10,19) = -1.0D0 
MATB(10) = 0.0DO 

SUM OF FORCES IN THE Y DIRECTION 
MATA(11,11) = 1.0D0 
MATA(11,14) = MASS2 
MATA(11,20) = -1.0D0 
MATB(11) = 0.0D0 

SUM OF FORCES IN THE Z DIRECTION 
MATA(12,12) = 1.0DO 
MATA(12,15) = MASS2 
MATA(12,21) = -1.0D0 

SUM OF FORCES LINK TWO EQUAL 
MATB(12) = -WG2 


EQUATIONS AT JOINT ONE 
IN THE & DIRECTION 


MATA(13,4 = =1,0D0 
MATA(13,8 = ee 
MATA(13,9 = RAG1(2 
MaTaats 23) = 1.0D0 
Maas 17) = ee 
MATA(13,18) = -RBG2(2 
MATB(13) Selb I= HIeZ 
ie tiie: DIRECTION 
MATA(14,5 =) 7 ODO 
MATA(14,7 = ene 
MATA(14,9 = -RAG1(1 
MATA(14,14) = 1.0D0 
MATA(14,16) = oes 
MATA(14,18) = RBG2(1 
MATB(14) Seeccr —oMICZ 
IN THE 2 DIRECTION 

MATA(15,6 = -1.0D0 

MATAGL >, 7 = eal 
MATA(15 3 = RAGI(1 
MatTAvi5, 15) = 1.0D0 
MATA(15,16) = ea 
MATA(15,17) = -RKBG2Z(1 
AGEs Set MKEZ 


SUM OF MOMENTS EQUATIONS FOR LINK TWO IN THE X,Y,2 DIRECTIONS 
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MATA(16,11) = RBG2(3 
MATA(16,12) = =REBG2Z(Z 
MATA( 16,16) =—= (2 
MATA( 16,17) = See (2 
MATA( 16,18) = eat 2 
MATA( 16,20) = -RAGZI3 
MATA(16,21) = RAG2(2 
MATB(16) = (-T1xX + T2X) * DEOS@RERZI) 
MATA(17,10) = -RBG2(3 
MATA( 17,12) =seeReGZicd 
MATA(17,16) = IXYT(2 
MATA(1? ,17) =S3 aT 
MATA( 17 (18) =) vZaZ 
MATA(17,19) = RAG2(3 
MATA(17,21) = -RAGZ(1 
MATB(17) Sec eodly +>22y) * DSiiaERzZi) 
MATA(18,9) = -1.0D0 
MATA(18,18) = 1.0D0 
MATB(18 = 02 0D90 
MATA(18,10) = RBG2(2 
MATA( 19,11) = =KBGZii 
MATA(I1S,160) = Tan 2) +er-Zns 
MATA(18,17) = Dygn2) + oiv2Zi3 
MATAC(1S, 18) = =i22ZaZ = i 22iS 
MATA(18,19) = -RAG2Z(2 
MATA(18,20) = RAG2(1 
MATB(18) = TiZeiee ce 


LINK THREE 


SUM OF FORCES 


peut ies 
LO AZe 


MATB(19) 


MATA 


IN THES DES ePION 


; 


1.0D0 
MASS3 


Q0.0D0 


SUMS OF FORCES ol yiieaal ERECTICN 


ney cee 
ZO 23 


MATB(20) 


MATA 


} 


1.0D0 
MASS3 


0.0D0 


SUM OF FORCES IN THE Z DIRECTION 


MATA 


a oe 
21,24 


} 


1.0D0 
MASS3 


MATB(21) = -WG3 
EQUATIONS AT JOINT TWO 
IN THE X DIRECTION 


MATA( ZZ 13) = =120D0 
MATA(22,17) = Ee 
MATA(22,18) = RAG2(2 
MATA(22,22) = 1.0D0 
MATA(22,26) = Seca 
MATA( 22,27) = =RBG3(Z 
MATB(22) = ee sr = 
IN THEVY DIRECTION 
MATA(23,14) = -1.0D0 
MATA(23,16) = ee 
MATA(23,18) = -RAG2(1 
MATA(23,23) = 1.QD0 
HATA( 23,25) = =RBG33 
MATA(23,27) = RBG3(1 
MATB (23) = MGs, = 


MIC4 


} 
} 


MJC4 
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‘e IN THE Z DIRECTION 











MATA(24,15) = -1.0D0 
MATA(24.16) = “BAG2(2 
MATA(24.17) = RAG2(1 
MATA(24.24) = 1.0D0 
MATA(24'25) = ea 
MATA(24.26) = -RBG3(1 
MATB (24) = MKC3 - MKC4 
x SUM OF MOMENTS EQUATIONS FOR LINK THREE IN THE X,Y,Z DIRECTIONS 
MATA(25,20) = RBG3(3 
MATA(25,21) = -RBG3(2 
MATA(25,25) = -IXXT(3 
MATA(25.26) = IXYT(3 
MATA(25,27) = IXzT(3 
MATB(25) = -T2X * DCOS(RLRZ1) 
MATA(26,19) = -RBG3(3 
MATA(26,21) = RBG3(1 
MATA(26,25) = IXYT(3 
MATA(26,26) = -IYYT(3 
MATA(26,27) = IYZT(3 
MATB(26) = -T2Y * DSIN(RLRZ1) 
MATA 27,9) = -1.0D0 
MATA(27,27) = 1.0D0 
MATB (275 = 0.0D0 
* MATA(27,19) = RBG3(2 
* MATA(27.20) = -RBG3(1 
x MATA(27.25) = IxXzT(3 
* MATA(27,26) = IYzT(3 
‘i MATA(27.27) = -1zzT(3 
x MATB (27) Sa ha 
ee. 10) jabs 
* INITIALIZE MATRIX ACCORDING TO CONSTRAINTS 
* CONSTRAINTS GROUP 1 WHEN ONLY LINK THREE IS IN MOTION 
* DO 118 I = 1,18 
* DO 18 J = 1,27 
* MATA 1,3) = 0.0 
x MATA(I.I) = 1.0 
x MATB(I) = 0.0 
*18 CONTINUE 
*118 CONTINUE 
* DO. le ieee. 27 
* DO 81 J = 1,18 
* MATA(I,J) = 0.0 
*81 CONTINUE 
*181 CONTINUE 
* GO TO 1111 
x CONSTRAINTS GROUP 2 WHEN LINK TWO AND THREE ARE IN MOTION 
* D0 19 I =1,9 
x DO 191 J = 1,27 
* MATA 1,3} = 0.0D0 
* MATA(I.I) = 1.0 DO 
fi MATB(I}) = 0.0D0 
x mATA 2-2 = 0.0D0 
* MATA(18.J) = 0.0D0 
x MATB 7 = 0.0D0 
* MATB(18 = 0.D0 
Pas 
* MATA(J,17) = 0.0D0 


7, Sa Si, a Sa, a, SO, A, SS Ma, SM, a a, SS, MM 


*78 


MATA( J, 18 e=menong 


TaN Gene = 1.0D0 

MATA(18,18) = 1.0D0 
CONTINUE 

CONTINUE 


DO 91 I = 10,27 
DO 92. J = ie 
MATA(I,J) = 0.0 
CONTINUE 
CONTINUE 
COme irr 


CONSTRAINTS GROUP 3 WHEN THREE OF THE LINKS ARE IN MOTION 
DO ie Jo= haz? 


ATA(7,J) = 0.0D0 
MATA(8.J) = 0.0D0 
MATA(J,7) = 0.0D0 
MATA(J.8) = 0.0D0 
MATB z = 0.0D0 
MATB(8 = 0.0D0 
MATA(17,J) = 0.0DO 
MATA(18,J) = 0.0D0 
MATA(J,17) = 0.0DO 
MATA(J,18) = 0.0D0 
MATB ae = 0.0D0 
MATB(18 = 0.0D0 
MATA(26,J) = 0.0D0 
MATA(27.J) = 0.0D0 
MATA(J,26) = 0.0D0 
MATA(J,27). = 0.0D0 
MATB = = 0.0D0 
MATB(27 = 0.0D0 
MATA eh = 1.0D0 
MATA(8,8 = 1.0D0 
MATA(17,17) = 1.0D0 
MATA(18,18) = 1.0D0 
MATA(26,26) = 1.0D0 
MATA(27.27) = 1.0D0 

CONTINUE 


* CONSTRAINT GROUP 4 THE FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIR. 


ee 


481 
48 


841 
84 


eee 


DO 48 I = 4,8 
DO 481 J = aa 


MATA 1,3} = 0.0D0 
MATA(I.I) = 1.0 DO 
MATB(I) = 0.0D0 
CONTINUE 
CONTINUE 


DO 84 I = 9,27 
DO 841 J = 4,8 : 
MATA(I,J) = 0.0 
CONTINUE 
CONTINUE 


CALL EQUATION SOLVER PROGRAM FROM IMSL 
CALL LEQT2F(MATA,M,N,IA,MATB,IDGT,WKAREA, IER) 


‘IF (IER .NE. 0) CALL ENDJOB 


FIND LCOGX,LCOGY,LCOGZ,THETA VALUES ,WX,WY,WZ 
LINK ONE 
AX1 = MATB(4) 
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606 
605 


VELX1 . a oan 

LCOGX1 = INTGRL(X1,VELX1) 

LCOGX(1) = LCOGX1 

AY1 = MATB(5) 

VELY1 = a Ooo 

LCOGY1 = INTGRL(Y1,VELY1) 

LCOGY(1) = LCcoGYl 

AZ1 = MATB(6) 

VELZ1 = se ee 

LCOGZ1 = INTGRL(Z1,VELZ1) 

LCOGZ(1) = LCOGZ1 

WD1X = MATB(7) 

W1X = INTGRL(O.,WD1X) 

WDX(1) = WD1X 

W1(1) = W1X 

WD1Y = MATB(8) 

W1Y = INTGRL(O.,WD1Y) 

WDY(1) = WD1Y 

W1(2) = W1Y 

WD1Z = MATB(9) 

W1Z = INTGRL(O.,WD1Z) 

WDZ(1) = WD1Z 

W1(3) = W1Z 
TRANSFORMATION MATRIX FROM EARTH FIXED TO 


sioneuecOR LENK ONE 
MATIR(1,1) = DCOS(RLRZ1) * DCOS(PTRY1) 


MAT1R(2,1) = DCOS(RLRZ1) * DSIN(PTRY1) 
DSIN(RLRZ1) * DCOS(YWRX1) 


MAT1R(3,1) = DCOS(RLRZ1) * DSIN(PTRY1) 
DSIN(RLRZ1) * DSIN(YWRX1) 


MAT1R(1,2) = DSIN(RLRZ1)*DCOS(PTRY1) 


MATIR(2,2) = DSIN(RLRZ1) * DSIN(PTRY1) 
DCOS(RLRZ1) * DCOS(YWRX1) 


MAT1R(3,2) = DSIN(RLRZ1) * DSIN(PTRY1) 
DCOS(RLRZ1) * DSIN(YWRX1) 


BODY FIXED COORDINATE 


* DSIN(YWRX1) -... 


PeDGOotyyRx1) +... 


* DSIN(YWRX1) +... 


x DECOS@awRKL) =... 


MAT1R(1,3) = -DSIN(PTRY1) 
MATIR(2,3) = DCOS(PTRY1) * DSIN(YWRX1) 
MATIR(3,3) = DCOS(PTRY1) * DCOS(YWRX1) 
GEietnem SUOCLTIZES OF LINK ONE IN BODY FIXED COORDINATE SYSTEM 


Dogces J = 1,3 
SUM1 = 0.0D0 
DOROOG Kk = le3 


SUM1 = SUM] + MATIR(J,K) * W1(K) 
CONTINUE 
BRATE1(J) = SUM1 


CONTINUE 


TRANSFORMATION MATRIX FROM BODY FIXED TO NON-ORTHOGONAL EULER 


COORDINATE SYSTEM FOR LINK ONE 


MATIT(1,1) = 0.0D0 

MATIT(2,1) = 1.0D0 

MAT1T(3,1) = 0.0D0 

MATIT(1,2) = ee EEE 

MAT1T(2,2) = DTAN(PTRY1) * DSIN(YWRX1) 
MAT1T(3,2) = 1.0D0/DCOS(PTRY1) * DSIN(YWRX1) 
MAT1T(1,3) = -DSIN YWRKL) 

MAT1T(2,3) = DTAN(PTRY1) * DCOS(YWRX1) 
MAT1T(3,3) = 1.D0/DCOS(PTRY1) * DCOS(YWRX1) 


Gl Pere bOCT Ti bomen alike cle IN The EULER COORDINATE SYSTEM 


fia 


DO 705 J —siF3 
SUM1 = 0.0D0 
DO 706 K = 1,3 
SUM1 = SUM + MATIT( JK) 4eepe tek) 


706 CONTINUE 
RATE1LX RATE1L 
INTEGRATION OF THE VELOCITIES OF LINK ONE IN EULER COOR. SYSTEM 
RLR2Z1 INTGRL -PI/2.,RATE1Z) 
PTCANY (1 PORY. * RADEG 





RATE1(J) = SUM1 
705 CONTINUE 
1 
RATELY = RATE1 A 
RATE1Z = RATE1(3 
YWRX1 = INTGRL(O.,RATE1X 
PTRY1 = INTGRL(0.,RATELY 
CONVERT THE ANGLES TO DEGREES 
prea (t = YWRX1 * RADEG 


bul 


ROLANZ(1) = RLRZ1 * RADEG 
GET THE DIRECTION COSINES FOR THE LINKVONE 


DRCSY(1) = DEOS (RLRZ1) * DSIN(PTRY1) * DCOS(YWRX1) +. 
DSIN(RLRZ1) * DSIN(YWRX1) 


DRCSX(1) = Dee) * DSIN(PTRY1) * DCOS(YWRX1) - 
DCOS(RLRZ1) * DSIN(YWRX1 ) 


DRCSZ(1) = DCOS(PTRY1) * DCOS(YWRX1) 


DRCRAX(1) = DACOS(DRCSX(1 
DRCRAY(1) = DACOS(DRCSY(1 
DRCRAZ(1) = DACOS(DRCSZ(1 
DRCANX(1) = DACOS(DRCSX(1)) * RADEG 
DRCANY(1) = DACOS(DRCSY(1)) * RADEG 
DRCANZ(1) = DACOS(DRCSZ(1)) * RADEG : 
LINK TWO 

AX2 = MATB(13 
VELX2 = oy ,AX2) 
LCOGK2. = INTGRL(X2 TEER?) 
HEOGK(2) = lees 
AY2 - one a 
VELY2 = anetate /AY2) 
LCOGY2 = INTGRL(Y2 FEY?) 
LGOG (2) = mege 
AZ2 = MATB(15) 
VELZ2 = HC 
LCOGZ25 = INterm( 22) van) 
LCOGZi 2) = heceze 
WD2X = MATB(16) 
W2X = INTGRL(O.,WD2X) 
WDX(2) = WD2X 
W2(1) = W2X 
WD2Y = MATB(17) 
W2Y = INTGRL(0O. ,WD2Y) 
WDY (2) = WD2Y 
W2(2) = W2Y 
WD2Z = MATB(18) 
W2Z = INTGRL(0.,WD2Z) 
WD2 (2) = WD2Z 

W2(3) = W2Z 


TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
SYSTEM FOR LINK TWO 


MAT2R(1,1) = DCOS(RLRZ2) * DCOS(PTRYZ2) 
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MAT2R(2,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) -... 
DSIN(RLRZ2) * DCOS(YWRX2) 


MAT2R(3,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) +... 
DSIN(RLRZ2) * DSIN(YWRX2) 


MAT2R(1,2) = DSIN(RLRZ2) * DCOS(PTRY2) 


MAT2R(2,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) +... 
DCOS(RLRZ2) * DCOS(YWRX2) 


MAT2R(3,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) -... 
DCOS(RLRZ2) * DSIN(YWRX2) 


MAT2R(1,3) = -DSIN(PTRY2) 

MAT2R(2,3) = DCOS(PTRY2) * DSIN(YWRX2) 

MAT2R(3,3) = DCOS(PTRY2) * DCOS(YWRX2) 

THE VELOCITIES OF LINK TWO IN BODY FIXED COORDINATE SYSTEM 


DORS07-) = 1,3 
SUM1 = 0.0D0 
DO 608 K = 1,3 
SUM1 = SUM1 + MAT2R(J,K) * W2(K) 
CONTINUE 
BRATE2Z(J) = SUM1 


607 CONTINUE 


a Gear 


708 


TRANSFORMATION MATRIX FROM BODY FIXED TO NON-ORTHOGONAL EULER 
COORDINATE SYSTEM FOR LINK TWO 


MAT 2T 
PALTZ. 
MAT ZT 


ape 
aE 
a2. 
MAT 2T 
MAIZT 


Hater 23) 


0.0D0O 
1.ODO 
0.O0DO 


Sad est 
DTAN(PTRY2) * DSIN(YWRX2) 
1.0D0/DCOS (PTRY2) DSIN(YWRX2) 


-DSIN a 
MAT 2T (2,3 DTAN(PTRY2) * DCOS(YWRX2) 
PRT ZT (3,3 1.0D0/DCOS(PTRY2) DCOS (YWRX2 ) 


THE VELOCITIES OF LINK TWO IN THE EULER COORDINATE SYSTEM 


DO 707 J = 1,3 
SUM1 = 0.0D0 | 
DO 708 K =1,3 
SUM1 = SUM1 + MAT2T(J,K) * BRATE2(K) 
CONTINUE | 
RATEZ(J) = SUMI1 


ot 





2, 
cee 
eZ 
22 
B52 





707 CONTINUE 


RATEZ 
RATEZ 
RATE2 


RATE2X% 
RATEZY 
RATE2Z 





1 
Z 
3 


= Peer tOneOr THESVELOCITIES OF LINK Two IN EVLER COOR. SYSTEM 


YWRX2 
FERYZ 
RLR2Z2Z 


INTGRE( 0] RATE ZY 
INTGRL(-PI/2.,RATE2Z) 


ENEGRE(O /RATE2¥| 


8 CONVERT- THE ANGLES TO DEGREES 


= GEL 


YAWANX 
PLCANY (2 PTRY2 * RADEG 
ROLANZ(2 RLRZ2 * RADEG 


THESOURECTION COSINES FOR THE LINK IWwO 


DRCSY(2) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRK2) -... 
DSIN(RLRZ2) * DCOS(YWRK2) 


DRCSX(2) = DSIN(RLRZ2) * DSIN(PTRY2)*DSIN(YWRK2) +... 
DCOS(RLRZ2) * DCOS(YWRX2) 


DRCSZ(2) = DCOS(PTRY2) * DSIN(YWRX2) 
DRCRAX(2) = DACOS(DRCSX(2)) 





A = YWRX2 * RADEG 
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BRCRAY (2) = tage? 
DRCRAZ(2) = DACOS(DRCSZ(2 
DRCANX(2) = DACOS(DRCSX(2)) * RADEG 
DRCANY(2) = DACOS(DRCSY(2)) * RADEG 
DRCANZ(2) = DACOS(DRCSZ(2)) * RADEG 
Jx1 = (L(1,1) +L 1, 2 * DCOS(DRCRAX(1 
SYi =) (eee * DCOS(DRCRAY (1 
3Z1 = (L(1,1) + L * DCOS(DRCRAZ(1 
* LINK THREE 
6 AX3 = MATB(22) 
VELX3 = ES See ,AX3) 
LCOGX3 = INTGRL(X3,VELK3) 
LCOGK(3) = Weeexe 
AY3 = MATB(23) 
VELY3 = Sa 
LCOGY3 = INTGRL(Y3,VELY3) 
LCOGY(3) = LCOGY3 
AZ3 = MATB(24) 
VELZ3 = INTGRL (0 ,AZ3) 
Ecocz: = INTGRL(2Z3,VELZ3) 
Lc0GZ(3) = Leaeze 
WD3X = MATB(25) 
W3X = INTGRL(0.,WD3X) 
WDX(3) = WD3X 
W3(1) = W3X 
WD3Y = MATB(26) 
W3Y = irern o. WD3Y) 
WDY (3) = WD3Y 
W3(2) = ve 
WD3Z = Enon) 
W3Z = INTCRE (0. /WD3Z) 
WDZ(3) = WD3Z 
W3(3) = W3Z 
* TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
x SYSTEM FOR LINK THREE 


MAT3R(1,1) = DEOS(RLRZ3) * DCOS(ETR as, 


MAT3R(2,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) -.. 
DSIN(RLRZ3) * DCOS (YWRX3 ) 


MAT3R(3,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) +... 
DSIN(RLRZ3) * DSIN(YWRK3) 


MAT3R(1,2) = DSIN(RLRZ3) * DCOS(PTRY3) 


MAT3R(2,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) +... 
DCOS(RLRZ3) * DCOS(YWRX3) : 


MAT3R(3,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) -... 
DCOS (RLRZ3) * DSIN(YWRX3) 


MAT3R(1,3) = -DSIN(PTRY3) 
- MAT3R(2,3) = DCOS(PTRY3) * DSIN(YWRX3) 
MAT3R(3,3) = DCOS(PTRY3) *DCOS(YWRX3) 
* GET THE VELOCITIES OF LINK THREE IN BODY FIXED COORDINATE SYSTEM 


DO 609 J =1,3 
SUM1 = 0.0D0 
DO 610 K = 1,3 
SUM1 = SUM1 + MAT3R(J,K) * W3(K) 


610 CONTINUE 
BRATE3(J) = SUM1 
609 CONTINUE 


+ 


TRANSFORMATION MATRIX FROM BODY FIXED TO NON ~ ORTHOGONAL EULER 
COORDINATE SYSTEM FOR LINK THREE 
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“ GET 


72ee 
709 


is INTEGRATION 


* CONVERT THE 


a GEL 


DYNAMIC 
NOSORT 


0.0D0 
1.0D0 
0.O0DO 


See cee 
DIAN (ERR) a DSIN (YWRX3 ) 
1.0D0/DCOS (PTRY3) DSIN(YWRX3 ) 


MAT3T SoS IN eS - 
MAT3T DTAN(PTRY3) * DCOS(YWRX3) 
iaten ers 1.0D0/DCOS(PTRY3) * DCOS(YWRX3) 


THE VELOCITIES OF LINK THREE IN THE EULER COORDINATE SYSTEM 


OG Geer a ale 
SUML = 0.0D0 
DO 710 K = 1,3 
SUM1 = SUM1 + MAT3T(J,K) * BRATE3(K) 


CONTINUE 
SUMI 
BALES 


RATE3(J) 
i 
2 
RATE3(3 


Shines PLOCTPI Es OF LINK THREE IN EULER COOR. SYSTEM 
INTGRL (0. /RATESY 


CONTINUE 


RATE3X 
Ree y 
RATE3Z 


RATE3 





YWRK3 
wars 
RLRZ3 


INTGRL(0.,RATE3Y 
INTGRL(-PI/2.,RATE3Z) 


ANGLES TO DEGREES 
YAWANX 
PTCANY 


3 YWRX3 * RADEG 
ROLANZ(3 


PTRY3 * RADEG 
Pacmeoteee! LON GOsTNES (POR IRE. LINK THREE 


RLRZ3 * RADEG 
DRCSY(3) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRK3) - 
DSIN(RLRZ3) * DCOS(YWRK3) 


DRCSXK(3) DSIN(RLRZ3 ) * DSIN(PTRY3) * DSIN(YWRK3) +. 
DCOS(RLRZ3) * DCOS(YWRX3) 


DRCSZ(3) = DCOS(PTRY3) * DSIN(YWRX3) 


DRCSK(3 
DRESY (3 
DRCS2Z(3 
Brest } 





DACOS 
DACOS 
DACOS 


DACOS 


DRCRAY (3 
DRCRAZ( 3 


DRCANX(3 
DRCANY (3 
DRCANZ(3 
JX1 
JY1 


JR2 
J2Z1 


JYZ 
JIX2 + 


JZZ 
TIPX 

ye 
JzZZ + 


Brenay(3| 





* RADEG 
* RADEG 
* RADEG 


} * Bcos  Brcaay 


DACOS (DRCSY 
DACOS (DRCSZ 


ICZ IL) ae 
JE G7e on oat Be 
Gi all 

c 








* DCOS(DRCRAY 
* DCOS(DRCRAZ 


vs BOS  DRCRAY 


+++ 


+ J, 











: 
3) 


, 


DCOS (DRCRAY 
DCOS (DRCRAZ 


ery 
TEE c 


sao 6 ADIN Co WD 


Z 
Z 
Us 


ey ota 
Be) ck 
Cok) te eb 











ee 
ed 
> as 


“a INPUT TORQUE AT JOINTS 


TOZ A * SIN 
tis =~ 10 sii 
T2x La SN 


END 
SLOP 
FORTRAN 


Nae LIME 
W * TIME 
W * TIME 
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+ + 
a9 [ha 9 Jig 8 
ba tian aatl 


* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 
SUBROUTINE CPROD(VECTA,VECTB,MI,MJ,MK) 
IMPLICIT REAL*8 (A-Z 
DIMENSION VECTA(3) (VECIrBIGS 











(3) 
al VECTA(2) * VECTB(3) - VECTA(3) * VECTB(2 
MJ = VECTA(3) * VECTB(1) - VECTA(1) * VECTB(3 
MK = VECTA(1) * VECTB(2) - VECTA(2) * VECTB(1 
RETURN 
END 
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APPENDIX C 


THREE DIMENSIONAL SIMULATION PROGRAM 
INVESTIGATION OF SINGULAR CONFIGURATION 


TERMINAL 
METHOD ADAMS 


PRINT 


-05,ERROR,ANG12Z ,ANG23Z 


CONTROL FINTIM = 4. OF DELMAX =-te mM DELERI = .O2 


SAVE 


OD Seno, ANG12X, ANG12Y ,ANG12Z , THETAB , THETAD , ANG23X,ANGZ3Y, 


ANG23Z, IYYT(2), irre) T2201 (2 


GRAPH 
GRAPH 
GRAPH 


EXCLUDE IA,IDGT, IER, i 


ARRAY 


ARRAY 
oo 


DE=TEKous le TEM TyYyYr(zZ) ,1X 


DE=TEK618 TIME ANG12X 
DE=TEK618) TIME, ANG12Y 
DE=TEK618 TIME , ANG12Z 
DE=TEK618) TIME ,ANG23X 
DE=TEK618) TIME,ANG23Y 
DE=TEK618) TIME,ANG23Z 
DE=TEK618) TIME, THETAB 


) X 
DIMENSION MATA(27 ( 
DIMENSION IXX(3,2),1XZ(3,2) 
DIMENSION mrs 3) ar 
DIMENSION MATIT 
INTEGER IER,I,J 


VECTA2(3) , VECTB2(3) 
nee eee O27 } 
TE2(3),BRATE3 (3) 


(2) ,HDZ(2) ,WKAREA(850) 
iver ( 3) 


-a-e.. 














DRCSX(3 DRCSY (3) .DRCSZ(3) 
DATA MATA/729 * 0. 


INITIAL 


* 


* 


* 


* 


INPUT PARAMETER CONSTANTS 
oe sate 


Q. 
Pl ya 2.000 
ODO” 


Q) ii il i 


Oo 


a 


t 

o it 
mPNO 
| i 

Nh 

~) 


QH =o Y 


INPUT JOINT LOCATIONS IN METERS 


JXO -ODO 
JYO .ODO 
JZO -QDO 
JX1 ODO 
epee .ODO 
JZ1 ODO 


USE THE NEAT SET OF JOINT TWO COORDINATES FOR CASE A 


JXZ Q0.0D0 
eZ ODO 
JZZ = 1.0D0 


USE THE NEAT SET OF JOINT TWO COORDINATES FOR CASE B 


rP-OddO00 
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YS a i a, Sa a 


> a a 


JX2Z2 = 1.0D0 
J¥Z = 0.0D0 
GZ25—= =i OD0 
USE THE NEXT SET OF JOINT TWO COORDINATES FOR CASE C 
JX2 = 0.0DO 
JY¥Z = 0.0D0 
JZZ2 = 2.0D0 


INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS FOR 
EACH DINK ENBS 





L(1,1) = 0.50D0 
Lil2) = oncom 
r(2,1) = 0es0no 
(2 208= 0.5000 
L(3,1) = G.50ne 
(3,2) = Oms0D0 
INPUT MASS AT LINK ENDS IN KILOGRAMS 
. MASS(1,1) = 2.5D0 
MASS(1,2) = 2.5D0 
MASS(2,1) = 2.5D0 
MASS(2,2) = 2.5D0 
MASS(3,1) = 2.5D0 
MASS(3,2) = 2.5D0 
INPUT OMEGA AND OMEGA DOT 
oye). 8) rl 
W1(I = 0.0D0 
W2(I = 0.0D0 
W3(I = 0.0D0 
WDX(I = 0.0D0 
WDY (I = 0.0D0 
WDZ(I = 0.0D0 
CONTINUE 
INPUT LOCATION OF LINK CENTERS OF GRAVITY 
LINK ONE 


LCOG 0 = Boe ODO 
X1 


ee 
tcocy (1) © = “3 
Yl = Lcoc Hd) 
LCOGZ(1) = o 
AL eS jeelee 371) 
NEXT SET FOR LINK TWO AND THREE TO USE FOR CASE A 


LCOGX(2) = ar EN 
LCOG ae 


.5D0 
y OGY (3) 
1.c0Gz(3) = 1.0D0 
23. = LCcoGZ(3) 
NEXT SET FOR i TWO AND THREE TO USE FOR CASE B 
LCOGK(2) = 0.5D0 
X2 LeocKt 2) 
LCOGY(2) = 0.0D0 
Y2 “=icocua 
LCOGZ(2) = 1. 39 
Z2 = LCO Z(2) 
LCOGX(3) = t 
x3 = LcoGK(3) 
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Ns Mr SP a SP MP a a 


218 
40 


60 
x 


64 
63 


INPUT MASS OF 


LCOGY(3) = 0. ODO 
et yeas) 
LCOGZ(3) = 1.0D0 
Zi «= LeoGz(3) 


NEAL SB tes ORw UN TWOmaN) THREE TO USE FOR@GASE C 


LCOGK(2) = 0.0D0 
Roan LCoGK) 
LCOGY(2) = 0.0D0 
v2. = LcoGy (2) 


=  LC0GzZ(3) 


5 ope 
5 .0DO 
5. 0D0 


WGl = MASS1%*G 
WG2 = MASS2*G 


mM 

5) 

NO 
| 


WG3 MASS3%*G 
AOX = 0.0D0 
AOY = 0.0D0O 
AOZ = 0.0D0 


DO 40 I = 1,27 
D 


EACH LINK IN KG AND COMPUTE WEIGHTS IN NEWTONS 


INPUT ACCELERATION OF JOINT ZERO 


INITIALIZE MATRIX A AND B TO ZERO 


Casoe = 1. 27 


MATA(I,J) = 
DQ(I) = 
MATC(I) = 
CONTINUE 
CONTINUE 


DO 60 I = 1,27 
MATB(I) = 0 
CONTINUE 


DO MGs ee 
D 


a ODO 
oy "ODO 


ODO 


TNITIALIZE TRE INITIAL VELOCITIES AND TRANSFORMATION MATRICIES 


0643 =1,3 


RATEZ 


Rage (1 
RATE3 


CONTINUE 
CONTINUE 


DERIVATIVE 
SEP ERRSEL (208,296,-1,1,1) 


NOSORT 


BEVELO = 0 


20D0 


OODODO000 DOD0O000 
© 
Oo 
© 


Corre Robt LEVEL, LEVLDO) 
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x INITIALIZE MATRIX A AND B TO ZERO 


DO 70st = tz, 
e) 


Lae 
MATA(I,J) = 

















0.0D0 
80 CONTINUE 
70 CONTINUE 
DOCOmDe= trae 
MATB(I) = 0.0D0 
HO(L) = 0. GD0 
90 CONTINUE 
x INPUT JOINT EQUATIONS 
* JOINT ZERO EQUATIONS 
x AB = AG1 + (WD1 X RB/Gl) + Wl X (Wl X RB/G1) 
VECTAO(1) = WDX(1 
VECTAO(2) = WDY(1 
VECTAO(3) = WDZ(1 
RBGI (1) =9ax0 = LCOGX(1 
RBG1(2) = JYO - LCOGY(1 
— -RBG1(3) = JZO - LcoGz(l 
CALL CPROD(VECTAO,RBG1,MIAO ,MJAO,MKAO) 
VECTAO(1) = W1(1 
VECTAO(2) = W1(2 
VECTAO(3) = W1(3 
CALL CPROD(VECTAO,RBG1,MIBO ,MJBO ,MKBO) 
VECTBO(1) = MIBO 
VECTBO(2) = MJBO 
VECTBO(3) = MKBO 


CALL CPROD(VECTAO,VECTBO,MICO,MJCO,MKCO) 


JOINT ONE EQUATIONS--- 
AA = AG1 + (WD1l X RA/G1) + Wl X (W1 X RA/G1) 





VECTA1(1) = WDX(1) . 
VECTA1(2) = WDY(1 
VECTA1(3) = WDZ(1 
RAGI(1) = JX1 - LCOGX(1 
RAG1(2) = JY1l - LCOGY(1 
RAG1(3) = JZ1 - LCOGZ(1 
CALL CPROD(VECTA1,RAG1,MIA1,MJA1 ,MKA1) 
VECTAL(1) = W1(1 
VECTA1(2) = W1(2 
VECTAL(3) = W1(3 
CALL CPROD (VECTA1,RAG1,MIB1,MJB1,MKB1) 
VECTB1(1) = MIB1 
VECTB1(2) = MJB1 
VECTB1(3) = MKB1 


CALL CPROD (VECTA1,VECTBI MICL MJICL) Mika) 


AB = AG2 + (WD2 X RB/G2) + W2 X (W2 X RB/G2) 





VECTA1(1) = WDX(2 

VECTA1(2) = WDY(2 

VECTA1(3) = WDZ(2 

RBG2(1) = JX1 - LCOGX(2 

ReG2(2Z) = JY¥1l = ECOG 

RBG2(3) = JZl - LCOGZ(2 

CALL CPROD (VECTA1,RBG2,MIA2 ,MJA2 ,MKAZ2) 

Beare = eel 

VECTA1(2) = W2(2 
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+ 


VECTA1(3) = W2(3) 
CALL CPROD (VECTA1,RBG2,MIB2,MJB2,MKB2) 


VECTB1( pee 0b 2 
VECTB1( 2) e=sIB2Z 
VECTB1(3) = MKB2 


CALL CPROD (VECTA1,VECTBI1 ,MIC2,MJC2,MKC2) 


JOINT TWO EQUATIONS 
AA = AG2 + (WD2 X RA/G2) + W2 X (W2 X RA/G2) 

















VECTA2(1) = WDK(2 
VECTA2Z(2) = WDY(2 
VECTA2(3) = WDZ(2 
RAGZ(1) = JX2 - LCOGX(2 
RAGZ(2) = J¥Z - LCOGY(2 
RAGZ(3) = JZ2Z2 = LCOGZ(2 
CALL CPROD (VECTA2,RAG2,MIA3,MJA3,MKA3) 
VECTAZ(1) = W2(1 
VECTA2Z(2) = W2(2 
VECTAZ(3) = W2(3 
CALL CPROD (VECTA2,RAG2,MIB3,MJB3,MKB3) 
WECTB2(1) = MIB3 
VECTB2(2) = MJB3 
VECIBZ 0s = — MNKES 


CALL CPROD(VECTA2,VECTB2,MIC3,MJC3,MKC3) 
AB = AG3 + (WD3 X RB/G3) + W3 X (W3 X RB/G3) 














VECTAZ(1) = WDX(3 
VECTAZ( Ze = TIDY (3 
VECTA2(3) = WDZ(3 
REGS( 1) = JAZ = sECOGK( 3 
BEGS(2) = Ji2 = peoGy( 3 
RBG3(3) = JZ2 - LCOGZ(3 
CALL CPROD (VECTA2,RBG3,MIA4,MKA4,MKA4) 
VECTAZ(1) = W3(1 
WECGTAZ( 2) = W3(Z 
VECTAZ(3) = W3(3 
CALL CPROD (VECTA2,RBG3,MIB4,MJB4,MKB4) 
VECTB2(1) = MIB4 
VECTB2(2) = MJB4 
VECTB2(3) = MKB4 


CALL CPROD (VECTA2,VECTB2,MIC4,MJC4,MKC4) 
SUM OF MOMENTS EQUATIONS 
DO 100 I = 1,3 


COMPUTE HX,HDOT X,HY,HDOT Y, HZ,HDOT Z 
Pecued) = nit 1) * DCOS( DRCRAX(1 
RK(IT.2) = L(I.2) * DCOS(DRCRAX(I 
RV(I.1) = -L(I.1) * DCOS(DRCRAY(T 
oe 2 =o. 2). * DCOS( DRERAY: 1 
PAT Ie =—= rn 1) * DCOS(DRCRAZ( I 
G71 2) = smn 2) * DCOS(DRCRAZ\1 
peat = res x ee ey * ea eeee A (Aen * Be ae 
Prete = MASS(1 2) = ((RY(1,2) * BY(1.2)) + (RZ(I,2) * RZ(L,2 
IXXT(I) = IXX(I,1) + IXX(1,2) 
IF (IXXT(I) .LE. .020) THEN 
TXATED) = .020 
ELSE 
IXXT(I) = IXXT(I) 
END IF 
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100 





1g (1.3) = MASS (74) * RX(I,1) * RY r.3) 
IxXY(I,2) = MASS(I,2) * RK(I.2) * RY(I.2 
IXYT(I) = IX¥(I,1) + IXY¥(T,2 
1xZ (1,1) = MASS (T, 1) APPT 1) RX(T, 1) 
IxZ(I,2) = MASS(I,2) * RZ(1I,2) * RX(T.2 
TRZT(1) = PRACT eee eee 
ee) = Ht Ee * 3) wogt?) * ANY - sede * Tee 
HDX(2) = WDX(2) * IXX(1,2)- WDZ(L) * IXZ(I.2) - WDY(I) * Ix¥(I/2 
1¥¥ (1,1) = MASS (T, 1) * extras) * R(T, 1}} 7 (Rz(T. 4) * ate 
Tyy(I,2) = MASS(I,2) * ((RK(I,2) * RX(I,2)) + (RZ(I,2) * RZ(I,2 
IYYT(I) = 1YY(I,1) + Ivy¥(I,2) 
IF (IYYT(I) .LE. .020) THEN 
IYYT(I) = .020 
ELSE 
IYYT(I) = IYYT(TI) 
END IF 
1¥2(1,2} = MASS (1.1) * RY(I,1) * Sat 
IyZ(I,2) = MASS(1I,2) * RY(I,2) * RZ(I,2 
IvYZT(I) = ryz(i,1) + ryz(r,2 
HDy (1) = wDy(T) * yy (1,2) - WOR(T} * yet, t) - — * v2 (1.1) 
HDY(2) = WDY(I) * Iyy(I,2) - WDX(I) * Ixv(I,2) - woz(T) * Iyz(I,2 
122 (7.0) = Mass (ted) * (RR(T ST) * ee + aes) * ea ee } 
IZZ(I,2) = MASS(I,2) * ((RX(I.2) * RX(I,2)) + (RY(I,2) * RY(T,2 
LZZT(1) =e) eer 2) 
IF (IZZT(I) .LE. .020) THEN 
IZZT(I) = 1020 
ELSE 
IZZT(1) = IZZT(T) 
END IF 
Hb2/ 1) = WDatT) * 122(tad) wouta) * 1xZ(2 J) - rit * ye ee 
HDZ(2) = WOZ(I) * IzZ(I,2) - WDK(I) * Ixz(I,2) - WDY(I) * Iyz(I,2 
- SUMHDX(I) = HDX(1) + HDX(2 
SUMHDY(I) = HDY(1) + HDY(2 
SUMHDZ(I) = HDZ(1) + HDZ(2 
CONTINUE 
ENTER CONSTANTS INTO MATRIX A 
LINK ONE | 
SUM OF FORCES IN THE X DIRECTION 
MATA 1.2} = 1.0D0 
MATA(1,4) = MASS1 
MATA(1,10) = -1.0D0 
SUM OF FORCES IN Y DIRECTION 
MATA(2,2) = 1.0D0 
MATA(2,5) = MASS1 
MATA(2,11) = -1.0D0 
SUM OF FORCES IN Z DIRECTION 
MATA 3.3) = 1.0D0 
MATA(3,6) = MASS1 
MATA(3,12) = -1.0D0 


EQUATIONS AT JOINT ZERO 
IN THE X DIRECTION 


MATA(4,4) = 1.QD0 
MATA(4,8) = ea 
MATA(4,9) = -RBG1(2 
IN THE Y DIRECTION 
MATA(5,5) = 1.0D0 
MATA(5,7) = ect 
MATA(5,9) = RBG1(1 
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” IN THE Z DIRECTION 





MATA(6,6) = 1.0D0 
MATA(6,7) = ee 
MATA(6,8) = -RBG1(1 

i SUM OF MOMENTS EQUATIONS FOR LINK ONE IN THE X,Y,Z DIRECTIONS 
MATA(7,2) = RBG1(3 
MATA(7,3) = -RBG1(2 
MATA(7,7) = -IXXT(1 
MATA(7,8) = IXYT(1 
MATA(7,9) = IXZT(1 
MATA 11d) = -RAG1(3 
MATA(7,12) = RAG1(2 
MATA(8,1) = -RBG1(3 
MATA(8,3) = RBG1(1 
MATA(8,7) = IXYT(1 
MATA(8,8) = ~IYYT(1 
MATA(8,9) = I¥ZT(1 
MATA(8,10) = RAG1(3 
MATA(8,12) = -RAG1(1 
MATA(9,1) = RBG1(2 
MATA(9,2) = -RBG1(1 
MATA(9,7) = IXZT(1) + IXZT(2) + IXZT(3 
MATA(9,8) = I¥ZT(1) + IYZT(2) + 1¥zZT(3 
MATA(9,9) = -I1ZZT(1) - IZZT(2) - 12Z2T(3 
MATA 3718) = -RAG1(2 
MATA(9,11) = RAG1(1 

LINK TWO 
SUM OF FORCES IN X DIRECTION 

MATA(10,10) = 1.0D0 
MATA(10,13) = MASS2 
MATA(10,19) = -1.0D0 

. SUM OF FORCES IN THE Y DIRECTION 
MATA(11,11) = 1.0D0 
MATA(11,14) = MASS2 
MATA(11,20) = -1.0D0 

i SUM OF FORCES IN THE Z DIRECTION 
MATA(12,12) = 1.0D0 
MATA(12,15) = MASS2 
MATA(12,21) = -1.0D0 

x EQUATIONS AT JOINT ONE 

x IN THE X DIRECTION 
MATA(13,4) = -1.0D0 
MATA(13,8) = “BAG! (3) 
MATA(13,9) = RAG1(2 
MATA(13,13) = 1.0D0 
MATA(13,17) = RBG2(3) 
MATA(13,18) = -RBG2(2 

x IN THE Y¥ DIRECTION 
MATA(14,5) = -1.0D0 
MATA(14,7) = RAGI(3) 
MATA(14,9) = -RAG1(1 
MATA(14,14) = 1.0D0 
MATA(14,16) = pees 
MATA(14,18) = RBG2(1 

x IN THE Z DIRECTION 
MATA(15,6) = -1.0D0 
MATA(15,7) = Bests 
MATA(15,8) = RAG1(1 
MATA “aR = 
MATA(18;16) = RBG2(2) 
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SUM OF MOMENTS EQUATIONS FOR LINK TWO IN THE X%,Y,Z DIRECTIONS 


MATA(15,17) = -RBG2(1) 


MATA(16,11) = RBG2(3 
MATA(16,12) = -RBG2(2 
MATA(16,16) = -IXXT(2 
MATA(16,17) = IXYT(2 
MATA(16,18) = IXZT(2 
MATA(16,20) = -RAG2(3 
MATA(16,21) = RAG2(2 
MATA(17,10) = -RBG2(3 
MATA(17,12) = RBG2(1 
MATA(17,16) = IxYT(2 
MATA(17,17) = -1¥YT(2 
MATA(17,18) = Iy¥zZT(2 
MATA(17,19) = RAG2(3 
MATA(17,21) = -RAG2(1 
MATA(18,10) = RBG2(2 
MATA(18,11) = -RBG2(1 
MATA(18,16) = IXZT(2) + IXZT(3 
MATA(18,17) = Iyzi(2) + IyzT(3 
MATA(18,18) = -1ZZT(2) - 122T(3 
MATA(18,19) = -RAG2(2 
MATA(18,20) = RAG2(1 

LINK THREE 

SUM OF FORCES IN THE X DIRECTION 
MATA (18.23) = 1 -0n 
MATA(19,22) = MASS3 

SUM OF FORCES IN THE Y DIRECTION 
MATA( 20.20) = 1.006 
MATA(20,23) = MASS3 

SUM OF FORCES IN THE Z DIRECTION 
MATA(21.21) = 1.0D0 
MATA(21,24) = MASS3 

EQUATIONS AT JOINT TWO 

IN THE X DIRECTION 
MATA(22,13) = -1.0D0 
MATA(22,17) = “AG2(3) 
MATA(22,18) = RAG2(2 
MATA(22,22) = 1.0D0 
MATA‘ 22,26) = REG3(3) 
MATA(22,27) = -RBG3(2 

IN THE Y DIRECTION 
MATA(23,14) = -1.0D0 
MATA(23,16) = RAG2(3 
MATA(23,18) = -RAG2(1 
MATA(23,23) = 1.0D0 
HATA\ 23,45) = ~BBGa(3) 
MATA(23,27) = RBG3(1 

IN THE Z DIRECTION 
MATA(24,15) = -1.0D0 
MATA‘ 24,18) = -RaG2(2) 
MATA(24,17) = RAG2(1 
MATA(24,24) = 1.0D0 
MATA(24,25) = RBG3(2 
MATA(24,26) = -RBG3(1 

SUM OF MOMENTS EQUATIONS FOR LINK THREE IN THE X,Y¥,Z DIRECTIONS 


MATA(25,20) = RBG3(3 
MATA(25,21) = -RBG3(2 
MATA(25,25) = -IXXT(3 
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Mara ese’ 338 = IXYT 

MATA(25,27) = IXZT(3 
MATA(26,19) = -RBG3(3 
MATA(26,21) = RBG3(1 
MATA(26,25) = IXYT(3 
MATA(26,26) = -IYYT(3 
Mema(26,27) = i273 
MATA(27,19) = RBG3(2 
MATA(27,20) = -RBG3(1 
Pee 27,25) =  IXZT13 
MATA(27,26) = IYZT(3 
MATA(27,27) = -IZZT(3 

x Go TO 1112 

x INITIALIZE MATRIX ACCORDING TO CONSTRAINT 

x CONSTRAINTS GROUP 1 WHEN ONLY LINK THREE IS IN MOTION 

x Pompe: = 1,18 

x pope g = 1727 

x MATA(I, a = 0n0 

* MATA(I,I) = 1.0 

x MATB(I} = 0.0 

*18 CONTINUE 

*118 CONTINUE 

x 

. Domrel b= 19), 27 

. Deo e we 1, 18 

x MATA(I,J) = 0.0 

*81 CONTINUE 

*181 CONTINUE 

x GO TO 1111 

* CONSTRAINTS GROUP 2 WHEN LINK TWO AND THREE ARE IN MOTION 

x pO 19 I =1,9 

x DO 191 J = 1,27 

. MATA $2} = 0.0D0 

. MATA(I,I) = 1.0 DO 

x MATB(IS = 0.0D0 

x 

x MATA a =n0 0D 

x MATA(18,J) = 0.0D0 

x MATB = O00 

MATB(18) = 0.DO 

* MATA(J,12) = 0.0D0 

: MATA(J,18) = 0.0D0 

x MATA( 17,17) = 1.0D0 

* MATA(18,18) = 1.0D0 

ras 

*191 CONTINUE 

x19 CONTINUE 

x 

x Bomoiet = 10,27 

x Do 92 J = 1,9 

x MamaA(t J) = 0.0 

*92 CONTINUE 

*91 CONTINUE 

. GO TO 1111 

x CONSTRAINTS GROUP 3 WHEN THREE OF THE LINKS ARE IN MOTION 

x 


Domeio.) = 1,27 


x MATA(7,J) = 0.0DO 
- MATA(8,J) = 0.0D0 
x MATA(J,7) = 0.0D0 
x MATA(J,8) = 0.0D0 
. MATB(7 = 10/00 
x MATB(8 = 0.0D0 
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Pee 


481 
48 


841 


84 


+e Fb Ft 





MATA(17,J) = 0.0DO 
MATA(18,J) = 0.0D0 
MATA(J,17) = 0.0D0 
MATA(J,18) = 0.0D0 
MATB i = 0.0D0 
MATB(18 = 0.0D0 
MATA(26,J) = 0.0D0 
MATA(27,J) = 0.0D0 
MATA(J,26) = 0.0D0 
MATA(J,27) = 0.0D0 
MATB a5 = 0.0D0 
MATB(27 = 0.0D0 
MATA bol = 1.0D0 
MATA(8,8 = L000 
MATA(17,17) = L2i0B0 
MATA(18,18) = 1.0D0 
MATA(26,26) = 1.0D0 
MATA(27,27) = 1.0D0 
CONTINUE 
CONSTRAINT GROUP 4 THE FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIR 
2 DO 48 I = 4,8 
DO 481 J = 1,27 
MATA sou = 0.0D0 
MATA(I,I) = 1.0 DO 
MATB(I = 0.0D0 
CONTINUE 
CONTINUE 
DO 84 I = 9,27 
DO 841 J = 4,8 
MATA(I,J) = 0.0 
CONTINUE 
CONTINUE 


USE THE FOLLOWING SET OF INFORMATION WHEN THE ANGULAR VELOCITY IS 
IN X DIRECTION REGARDLESS OF THE INITIAL CONFIGURATION 


ENTER THE THEORITICAL VALUES ASSUMING THE LINK TWO AND THREE ARE 
IN PLANAR MOTION AND ANGULAR VELOCITY IS IN THE X DIRECTION 





LINK TWO 
THEORITICAL ANGULAR VELOCITIES (APPLIED IN THE X DIRECTION) 
MATB(18) = 0.0DO 
MATB(17) = 0.0DO 
MATB(16) = -((PI**3) / 8.0D0) * DSIN(W * TIME) 
THDDOT = MATB(16) 
THTDOT = Se eee , THDDOT ) 
THETRB = INTGRL(0O.,THTDOT) 
THETAB = THETRB * RADEG 
LINEAR VELOCITIES 
MATB(15) = ~ ae x aa + ape zx x x eek 
MATB(14) = (THDDOT * RBG2(3 + (THTDOT ** 2) * RBG2(2 
MATB(13) = 0O.ODO 
LINK THREE 
ANGULAR VELOCITIES 
MATB(27) = 0.0DO 
MATB(26) = 0.0DO 
MAte( 25) = 0. ODO 
LINEAR VELOCITIES 
MATB(24) = MATB(15 a Se ao Eee eee 
MATB(23) = MATB(14)-(THDDOT*RAG2 (3) )-( THTDOT**2 ) *(RAG2(2 
MATB(22) = MATB(13 








END OF THE INFORMATION FOR XK DIRECTION 
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USE THE FOLLOWING SET OF INFORMATION WHEN THE ANGULAR VELOCITY IS 
IN THE Y DIRECTION REGARDLESS OF THE INITIAL CONFIGURATION 


ENTER THE THEORITICAL VALUES ASSUMING THE LINK TWO AND THREE ARE 
IN PLANAR MOTION AND ANGULAR VELOCITY IS IN THE Y DIRECTION 


LINK TWO 
THEORITICAL ANGULAR VELOCITIES(APPLIED IN THE Y DIRECTION ) 
MATB(18) = 0.0DO 
MATB(17) = ((-PI**3)/8)*SIN(W*TIME) 
MATB(16) = 0.ODO 
THDDOT = MATB(17) 
THTDOT = INTGRE (Or ETRE /THDDOT) 
THETRB = INTGRL(O INTGRL(31*#2/4. ,THDDOT) 
THETAB = THETRB * RADEG 
LINEAR VELOCITIES 
MATB(15) = (THDDOT * RBG2(1)) + (THTDOT ** 2) * RBG2(3) 
MATB(14) = ODO 
MATB(13) = ~(THDDOT * RBG2(3)) + (THTDOT ** 2) * RBG2(1) 
LINK THREE 
ANGULAR VELOCITIES 
MATB(27) = 0.0DO 
MATB(26) = 0.0DO 
MATB(25) = 0.0DO 
LINEAR VELOCITIES 
MATB(24) = MATB(15)-(THDDOT*RAG2(1))-(THTDOT**2)*(RAG2(3) ) 
MATB(23) = MATB(14 
MATB(22) = MATB(13)+(THDDOT*RAG2 (3) )-(THTDOT**2)*(RAG2(1)) 





END OF THE INFORMATION FOR THE Y DIRECTION 


USE THE FOLLOWING SET OF INFORMATION WHEN THE ANGULAR VELOCITY IS 
IN THE Z DIRECTION REGARDLESS OF THE INITIAL CONFIGURATION 


ENTER THE THEORITICAL VALUES ASSUMING THE LINK TWO AND THREE ARE 
IN PLANAR MOTION AND ANGULAR VELOCITY IS IN THE Z DIRECTION 


LINK TWO 
THEORITICAL ANGULAR VELOCITIES(APPLIED IN THE Z DIRECTION ) 
MATB(16) = 0.0D0 
MATB(17) = 0.0D0 
MATB(18) = -((PI**3) / 8.0D0) * DSIN(W * TIME) 
THDDOT = MATB(18) 
THIDOT = INTGRE S. STHFDOE) , THDDOT) 
THETRB = INTGRL(O.,THTDOT) 
THETAB = THETRB * RADEG 
LINEAR VELOCITIES 
MATB(14) = a tee x LAER + fee eK 2) x a 
MATB(13) = (THDDOT * RBG2(2)) + (THTDOT ** 2) * RBG2(1 
MATB(15) = 0.O0DO 
LINK THREE 
ANGULAR VELOCITIES 
MATB(27) = 0.0DO 
MATB(26) = 0.0DO 
MATB(25) = 0.0DO 
LINEAR VELOCITIES 
MATB(24) = MATB(15 
MATB(23) = MATB(14 207+ 12 )7 1BRG2 (2) ) 
MATB(22) = MATB(13)-(THDDOT*RAG2(2) )-(THTDOT**2)*(RAG2(1 





END OF foe INFORMATION FOR THE Z DIRECTION 


NEAT SET OF STATEMENTS ARE COMMON IN ANY PLANAR MOTION AND THEY ARE 
IN YHE CODE IN EVERY CAS2. THESE TERMS ARE ACCELERATION OF THE LINK 
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a 55 
505 


ONE AND FORCES AT EACH JOINT 
LINK ONE LINEAR AND ANGULAR ACCELERATIONS 








MATB(4) = 0.0D0 
MATB(5) = 0.0D0 
MATB(6) = 0.0D0 
MATB(7) = 0.0DO 
MATB(8) = 0.0D0 
| MATB(9) = 0.0D0 
FORCES 
JOINT TWO 
MATB(21) = -MASS3 * MATB(24) - WG3 
MATB(20) = -MASS3 * MATB(23 
MATB(19) = -MASS3 * MATB(22 
JOINT ONE 
MATB(12) = MATB(21) - MASS2 * MATB(15) -WG2 
MATB(11) = MATB(20) - MASS2 * MATB(14 
MATB(10) = MATB(19) - MASS2 * MATB(13 
JOINT ZERO 
MATB(3) = MATB(12) - MASS1 * MATB(6) -WGl 
MATB(2) = MATB(11) - MASS1 * MATB(S 
MATB(1) = MATB(10) - MASS1 * MATB(4 





END OF THE INFORMATION 
MULTIPLY MATA AND MATB 


DO 505 J = 1,27 
SUM1 = 0.0 
DO: S350 = 2? 
SUM1 = SUM1 + MATA(J,K) * MATB(K) 


CONTINUE 
DO(J) = SUM1 
CONTINUE 
DO 506 I =1, 


MATC(I) = $5 0(1) 


506 CONTINUE 


CALL EQUATION SOLVER PROGRAM FROM IMSL 

CALL LEQT2F(MATA,M,N,IA,DQ,IDGT,WKAREA, IER) 
IF (IER :NEw 0) GALL ENDIOS 
FIND LCOGX,LCOGY,LCOGZ,THETA VALUES ,WX,WY ,WZ 


LINK ONE 
AX1 = p0(4) 
VELX1 = st AER GE) 
LCOGX1 = INTGRL(X1,VELX1) 
neoch(ae = — 
AY1 = D9(5) 
VELY1 = et ee /AY1) 
LCOGY1 = INTGRL(Y1,VELY1) 
ECOGwCi = mie 
AZ1 = Do(6 ) 
VELZ1 = eee ges Ze) 
LCOGZ1 = INTGRL(Z1 ,VELZ1) 
Veoez( il). = heocr 
WD1X = Do(7) 
W1X = INTGRL(0.,WD1X) 
WDX(1) = WD1X 
Wi(1) = W1X 
WD1Y = DQ(8) 
W1Y = Therm one ybise 
WDY(1) = WD1Y 
W1(2) = W1Y 
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705 


WD1Z = poe 

W1Z =I TGRL (0. ,WD1Z) 

WDZ(1) = WD 

W1(3) = WIZ 
TRANSFORMATION 


SYSTEM FOR THE LINK ONE 


MAT1R(1,1) = DCOS(RLRZ1) * DCOS(PTRY1) 


MATIR¢Z,1) = Bere renee * DSIN(PTRY1) 
DSIN(RLRZ1) * DCOS(YWRX1) 


MAT1R(3,1) = DCOS(RLRZ1) * DSIN(PTRY1) 
DSIN(RLRZ1) * DSIN(YWRX1) 


MAT1R(1,2) = DSIN(RLRZ1) * DCOS(PTRY1) 


MATIR(2,2) = DSIN(RLRZ1) * DSIN(PTRY1) 
DCOS(RLRZ1) * DCOS(YWRX1) 


MAT1R(3,2) = DSIN(RLRZ1) * DSIN(PTRY1) 
DCOS (RLRZ1) * DSIN(YWRX1) 


MATIR(1,3) = -DSIN(PTRY1) 
MAT1R(2,3) = DCOS(PTRY1) * DSIN(YWRX1) 
MAT1R(3,3) = DCOS(PTRY1) * DCOS(YWRX1) 








x 


* 


* 


* 


MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 


DSIN(YWRK1) -... 


DeGstwaRxl) +... 


DSIN(YWRK1) +... 


DCOS(YWRK1) -.. 


GET THE VELOCITIES FOR LINK 1 IN BODY FIXED COOR. SYSTEM 
DO 605 J = 1,3 
SUM1 = 0.0D0 
DO 606 K = 1,3 
SUM1 = SUM1 + MATIR(J,K) * W1(K) 
CONTINUE 
BRATE1 (J) = SUM1 
CONTINUE 
TRANSFORMATION MATRIX FROM BODY FIXED TO EULER COORDINATE 
SYSTEM FOR THE LINK ONE 
MAT) = 0.00 
MATLIT 2, Dy) = 15000 
MAT1T(3,1) = 0.0D0 
MATLT - = Bees (ee 
MATIT(2,2) = DTAN(PTRY1) * DSIN(YWRX1) 
MAT1T(3,2) = 1.0D0/DCOS(PTRY1) * DSIN(YWRX1) 
MAT1T(1,3) = -DSIN rey 
MAT1T(2,3) = DTAN(PTRY1) * DCOS(YWRX1) 
MAT1T(3,3) = 1.DO0/DCOS(PTRY1) * DCOS(YWRX1) 
GET THE YAW,PITCH AND THE ROLL RATES FOR LINK ONE 


DOwiOS J-= 1,3 
SUM1 = 0.0D0 
DeeiGe Kk = 173 


SUM1 = SUM1 + MATIT(J,K) * BRATE1(K) 











CONTINUE 
RATE1(J) = SUM1 
CONTINUE 
RATELX = RATE1(1 
RATELY = RATE1(2 
RATE1Z = RATE1(3 
YWRX1 = INTGRL(O. ,RATE1X 
PTRY1 = INTGRL(0.,RATELY 
BURZI s= INTGRE(O. ,RATE1IZ 
YAWANX(1) = YWRKX1 * RADEG 
PTCANY(1) = PTRY1 * RADEG 
ROLANZ(1) = RLRZ1 * RADEG 








DIRECTION COSINES FOR THE FIRST LINK SAME FOR EACH CASE 
DRCSY(1) = DCOS (RLRZ1 ) ae ae * DCOS(YWRX1) +. 


DSIN(RLRZ1) * DSIN(YWRX1 


as 


+ 


+ + 


DRCSX(1) = DSIN(RLRZ1) * DSIN(PTRY1) * DCOS(YWRX1) -... 
DCOS(RLRZ1) * DSIN(YWRX1) 


DRCSZ(1) = DCOS(PTRY1) * DCOS(YWRX1) 
GET THE ANGLES AS RADIANS 








DRCRAX(1) = DACOS(DRCSX(1 
DRCRAY(1) = DACOS(DRCSY(1 
DRCRAZ(1) = DACOS(DRCSZ(1 

CONVERT THE DIRECTION COSINES TO DEGREES 
DRCANX(1) = DACOS(DRCSX(1)) * RADEG 
DRCANY(1) = DACOS(DRCSY(1)) * RADEG 
DRCANZ(1) = DACOS(DRCSZ(1)) * RADEG 

LINK TWO | 

Ax = DQ(13) 
VELX2 = aL (Ole 
meOGXZ = INTGRL(X2,VELX2) 
ECOGK(Z) = TeoG 
AY2 = D9 (14) 
VELY2 = fj Tap sent) 
LCOGY2 = INTGRL(Y2,VELY2) 
LCOGY(2) = Heecyz 
AZ2 = bo 
VELZ2 = Ay. 
LGoGaz = INTGRL(Z2,VELZ2) 
HCOGZ(2)u = beeGze 
WD2X = DQ(16) 
W2X = INTGREC (P1442) (4.0 ype) 


USE THE INIT. COND. WITH ONLY WHICHEVER VELOCITY APPLIED 
AND KEEP THE TWO OTHER ANG. VEL. INIT. COND. AS ZERO 


USE THE NEXT STATEMENT IF THE ANGULAR VELOCITY IS IN THE X DIR. 


THETRD = INTGRL(O. ,W2X) 
WDX(2) = WD2X 
W2(1) = W2X 
WD2Y = DO(17) 
W2Y = INTGRL(O. ,WD2Y) 
USE THE NEXT STATEMENT IF THE ANGULAR VELOCITY IS IN THE Y DIR. 
THETRD = INTGRL(0.,W2Y) 
WDY (2) = WD2Y 
W2(2) = W2Y 
WD2Z 


Be) 
W2Z INTGRL(0.,WD2Z) 
USE THE NEXT STATEMENT IF THE ANGULAR VEEnOGiT yStseitN THis Zeb ine 


THETRD = INTGRL(0.,W2Z) 
WDZ(2) = WD2Z 
W2(3) = W2Z 
THETAD = THETRD * RADEG 
ERROR = ABS(((THETAD-THETAB)/180.) * 100) 


TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
SYSTEM SPOR THE LCilikeano 


MAT2R(1,1) = DCOS(RLRZ2) * DCOS(PTRY2) 


MAT2R(2,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRK2) -... 
DSIN(RLRZ2) * DCOS (YWRX2) 


MAT2R(3,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) +... 
DSIN(RLRZZ) * DSIN(YWRX2) 


MAT2R(1,2) = DSIN(RLRZ2) * DCOS(PTRY2) 
MATZR(2,2) = DSIN(RLRZZ)) AeDSmn( ETRY 2 = DSMiwk2) oo, 
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DCOS(RLRZ2) * DCOS(YWRX2) 


MAT2R(3,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) -... 
DCOS(RLRZ2Z) * DSIN(YWRX2) 


MAT2R(1,3) = -DSIN(PTRY2) 
MAT2R(2,3) = DCOS(PTRY2) * DSIN(YWRX2) 
MAT2R(3,3) = DCOS(PTRY2) * DCOS(YWRX2) 
GET THE VELOCITIES FOR LINK 2 IN BODY FIXED COOR. SYSTEM 


DO 607 J = 1,3 
SUM1 = 0.0D0 
DO 608 K = 1,3 
SUM1 = SUM1 + MAT2R(J,K) * W2(K) 
CONTINUE 
BRATE2(J) = SUM1 
CONTINUE 


TRANSFORMATION MATRIX FROM BODY FIXED TO EULER COOR. SYSTEM 
FOR THE LINK TWO 


il 








Metacil; 1) = @.0D0 

MATZT(2,1) = 1.0D0 

Mar Zr sl) = 0.0D0 

MATZT(1,2) = Bee ohnaa. 

MAT2T(2,2) = DTAN(PTRY2) * DSIN(YWRX2) 

mel o,2) = i20be/DCOS(PTRYZ> DSIN(YWRX2Z) 
MAT2T(1,3) = San eee 

MAT2T(2,3) = DTAN(PTRY2) * DCOS(YWRX2) 
Matzi(3s,3) = 1.0D0/DCOS(PTRYZ) DCOS (YWRX2 ) 


GET THE YAW,PITCH AND THE ROLL RATES FOR LINK TWO 


BO 707 J = 1,3 
SUM1 = 0.0D0O 





CONTINUE 
RATEZXK RATEZ 
USE GtigeheAt THREE STATEMENTS FOR CASE A 
RLRZ2Z INTGRL(-PI/2.,RATE2Z) 
Pir TIPCRL On RAGE ZY 


DO 708 K = 1,3 
SUM1 = SUM1 + MAT2T(J,K) * BRATE2(K) 
RATE2(J) = SUM1 
CONTINUE 
1 
RATE2Y = RATE2(2 
RATE2Z = RATE2(3 
YWRX2 = INTGRL O;,RATE2E) 
PTRY2 = INTGRL(O. ,RATE2Y 
USE THE NEXT THREE STATEMENTS FOR CASE B 
YWRX2 = INTGRL(O.,RATE2X 
RLRZ2 = INTGRL(PI/2.,RATE2Z) 
USE THE NEXT THREE STATEMENTS FOR CASE C 





YWRAZ = INTGRL(OQ. ,RATEZX 
bere. = INTGRE(O. ,RATEZY 
RLRZZ = INTGRL(O. ,RATEZZ 





RATEZ(1) = RATEZK 
Z RAGEZ © 
RATEZ(3 RATE2Z 


YAWANX(2) = YWRX2 * RADEG 
PTCANY(2) = PTRY2 * RADEG 
ROLANZ(2) = RLRZ2 * RADEG 





USEeEHEeNEXT SET OF THE DIRECTION COSINES FOR LINK TWO FOR CASE A 


DReESY(2) = DCOS(RLRZZ) * DSIN(PTRY2Z) * DSIN(YWRK2) -... 
DSIN(RLRZ2) * DCOS(YWRX2) 


DRCSX(2) = DSIN(RLRZ2) * DSINCPTRY2) * DSIN(YWRX2) +... 


oF 


YS, i, a a a a SI a a 


DCOS (RLRZ2) * DCOS (YWRX2) 
DRCSZ(2) = DCOS(PTRY2) * DSIN(YWRX2) 
USE THE NEAT SET OF THE DIRECTION €OSTNES FOR Elite liGeaok CASE E 


DRCSY(2) = DCOS(RLRZ2) * DCOS(PTRY2) 
DRCSX(2) = DSIN(RLRZ2)*DCOS(PTRY2) 


DRCSZ(2) = -DSIN(PTRY2) 

USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK TWO FOR CASE C 
DRCSY(2) = DCOS(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) +... 
DSIN(RLRZ2) * DSIN(YWRX2) 
DRCSX(2) = DSIN(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) -... 
DCOS(RLRZ2) * DSIN(YWRX2) 
DRCSZ(2) = DCOS(PTRY2) * DCOS(YWRX2) 

GET THE ANGLES AS RADIANS 





DRCRAX(2) = DACOS(DRCSX(2 
DRCRAY(2) = DACOS(DRCSY(2 
DRCRAZ(2) = DACOS(DRCS2Z(2 

CONVERT THE DIRECTION COSINES TO DEGREES 
DRCANX(2) = DACOS(DRCSX(2 * RADEG 
DRCANY(2) = DACOS(DRCSY(2 * RADEG 
DRCANZ(2) = DACOS(DRCSZ(2 * RADEG 








FIND THE JOINT LOCATION 





3X1 = (L(1,1) + L(1,2)) * DCOS(DRCRAX(1 
Jv¥i = (L(l,i) + L(1,2)) * Dcos(DRCRAY(1 
JZi = (L(1,1) * L(11,2)) * DeOS(DRenaa 1 
LINK THREE 
AX3 = DO(22) 
VELX3 = es er) 
LCOGX3 = INTGRL(X3,VELX3) 
ECOGK(3) = Geacrs 
AY3 = BO(23) 
VELY3 - INTGRE (0. AYS) 
LCOGY3 = INTGRL(Y3,VELY3) 
LCOGY(3) = LCOGY3 
AZ3 = DO (24) 
VELZ3 =] TGR} (0. 223) 
LCOGZ3 = INTGRL(Z3,VELZ3) 
LCOGZ(3) = LCoGz3 
WD3X = DQ(25) 
W3X = INTGRL(0.,WD3X) 
WDX(3) = WD3X 
W3(1) = W3X 
WD3Y = DOQ(26) 
W3Y = INTGRE (0. ,WD3Y) 
WDY (3) = WD3Y 
W3(2) = W3Y 
WD3Z = DQ(27) 
W3Z = INTCRL (0. ,WD3Z) 
WDZ (3) = WD3Z 
W3(3) = W3Z 


TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COOR. SYSTEM 
FOR THE LINK THREE 


MAT3R(1,1) = DCOS(RLRZ3) * DCOS(PTRY3) 


MAT3R(2,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) -... 
DSIN(RLRZ3) * DCOS(YWRX3) 


MAT3R(3,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) +... 
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DSIN(RLRZ3) * DSIN(YWRX3) 
MAT3R(1,2) = DSIN(RLRZ3) * DCOS(PTRY3) 


MAT3R(2,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) +... 
DCOS(RLRZ3) * DCOS(YWRX3) 


MAT3R(3,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) -... 
DCOS(RLRZ3) * DSIN(YWRX3) 


MAT3R(1,3) = -DSIN(PTRY3) 
MAT3R(2,3) = DCOS(PTRY3) * DSIN(YWRX3) 
MAT3R(3,3) = DCOS(PTRY3) * DCOS(YWRX3) 


GET THE VELOCITIES FOR LINK 3 IN BODY FIXED COOR. SYSTEM 
DO 609 J = 1,3 
SUM1 = 0.0D0 
DO 610 K = 1,3 
SUM] = SUM1 + MAT3R(J,K) * W3(K) 
CONTINUE 
BRATE3(J) = SUM1 
CONTINUE 


TRANSFORMATION MATRIX FROM BODY FIXED TO EULER COOR. SYSTEM 


FOR Gee LINK THREE 


MATST(t, 1) = OF0Lg 

Hatst( 2/1} = 1.0D0 

Meisl(3,1) = O7ene 

MAT3T(1,2) = DCOS HEB 

MAT3T(2,2) = DTAN(PTRY3) * DSIN(YWRX3) 
Melst(3,2) = L.0pe@/DECUSCETRYS) DSIN(YWRX3) 


= Tran ees 
= DTAN(PTRY3) * DCOS(YWRX3) 


Mearol (2,3 
1.0D0/DCOS (PTRY3) DCOS (YWRX3 ) 


Hatst\ 2/3 
MATST(3,3 


Boe 709 J 


GET THE YAW,PITCH AND THE ROLL RATES FOR LINK THREE 
= 1 
) 


SUM1 = ODO 
DO 710 K = 1,3 
SUM1 = SUM1 + MAT3T(J,K) * BRATE3(K) 
CONTINUE 


RATE3(J) = SUM1 


CONTINUE 


USE 


RATE3X 
RATE3Y RADES<( 2 
RATE3Z RATE3 (3 


PRE VNEXT THREE FOR. THE CASE A 


YWRA3 INTGRL(O.,RATE3X 
biReS INTGRL(O. ,RATE3Y 
RLRZ3 INTGRL(-PI/2.,RATE3Z) 


Rate (2) 





USE THE 


YWRX3 
PIRYS 
RLRZ3 


USE THE 


YWRX3 
PIRYS 
RLRZ3 


NEXT THREE FOR THE CASE B 


INTGRL(Q. ,RATE3X 
INTGRL(Q.,RATE3Y 
INTGRL(PI/2.,RATE3Z) 


NEXT THREE FOR THE CASE C 
INTGRL\O/ Rages 


INTGRL(O.,RATE3Y 
DNTGRE(O. (RaTESZ 





PTCANY (3 PTRY3 * RADEG 
ROLANZ(3 RLRZ3 * RADEG 


Peau (3| = YWRX3 * RADEG 


Useeneeen. SEL. OF THE DEREGTION COSINES FOR LINK THREE FOR CASE A 


DRCSY(3) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) -... 
DSIN(RLRZ3) * DCOS(YWRX3) 


ao 


DRCSX(3) = DSIN(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3)+... 
DCOS(RLRZ3) * DCOS(YWRX3) 


DRCSZ(3) = DCOS(PTRY3) * DSIN(YWRX3) 


USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK THREE FOR CASE B 
DRCSY(3) = DCOS(RLRZ3) * DCOS(PTRY3) 
DRCSX(3) = DSIN(RLRZ3)*DCOS(PTRY3) 


DRCSZ(3) = -DSIN(PTRY3) 
USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK THREE FOR CASE C 


DRCS¥(3) = DCOS(RLRZ3) * DSIN(PTRY3) * DCOS(YWRK3)+... 
DSIN(RLRZ3) * DSIN(YWRX3) 


DRCSX(3) = DSIN(RLRZ3) ayo ee * DCOS (YWRX3)-.. 


DCOS(RLRZ3) * DSIN(YWRX3 

DRCSZ(3) = DCOS(PTRY3) * DCOS(YWRX3) 
GET THE ANGLES AS RADIANS 

Breas) Bacos pRcst( 3) 


Pi i, i, a, i, a, Se, a, 


DRCRAY (3 DACOS (DRCSY(3 
DRCRAZ(3 DACOS (DRCS2Z(3 


* CONVERT THE DIRECTION COSINES TO DEGREES 
Brea 3) - Dacos{ Dacst 3 * RADEG 


DRCANY (3 DACOS (PRES (3 * RADEG 
DRCANZ(3) = DACOS(DRCSZ(3 * RADEG 


* FIND ANGLE BETWEEN LINK 2 AND LINK 3 TO CHECK IF THE ARM LINKS 
* are PASSING THROUGH THE SINGULAR POINTS 


ANG23X DECAY 2 = Brean 3) 








ANG23Y = DRCANY(2) - DRCANY(3 
ANG23Z DRCANZ(2) - DRCANZ(3 


ANG12X DRA} - DRCANX A 
2 


ANG12Y = DRCANY(1) - BRAN 
r * BCOS  BRCRAL 
2 
2 
Z 





ANG12Z DRCANZ(1) - DRCANZ 
= FIND THE JOINT LOCATION 

* DCOS (DRCRAY 
* DCOS (DRCRAZ 


JX2 JAL + “Gi 1 
JYL + Cie: 
; “ Bcos (BRCRAY 








JY2 
JZ2 JZL + (ia 
sis OZ sree 
JYZ +a: * DCOS(DRCRAY 
IZ2ea 3) DRCRAZ 


EY. 


Z 
2 
2 
DEEZ | 


} 
3) 


rere 
ba ctl tie il 


* DCOS 


END 
STOP 
FORTRAN 


“ SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 


SUBROUTINE CPROD(VECTA,VECTB,MI,MJ,MK) 
IMPLICIT REAL*8 (A-Z) 
eye fee. VECTA(3) , VECTB(3) 


eae VECTA(2) * VECTB(3) - VECTA(3) * VECTB(2 
MJ = VECTA(3) * VECTB(1) ~ VECTA(1) * VECTB(3 
MK = VECTA(1) * VECTB(2) - VECTA(2) * VECTB(1 
RETURN 
END 
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